CFeatureExtraction_ORB.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, 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::utils;
using namespace mrpt::vision;
using namespace mrpt::system;
using namespace std;

/************************************************************************************************
*                                                               extractFeaturesORB
**
************************************************************************************************/
void CFeatureExtraction::extractFeaturesORB(
        const mrpt::utils::CImage& inImg, CFeatureList& feats,
        const unsigned int init_ID, const unsigned int nDesiredFeatures,
        const TImageROI& ROI) const
{
        MRPT_UNUSED_PARAM(ROI);
        MRPT_START

#if MRPT_HAS_OPENCV
#if MRPT_OPENCV_VERSION_NUM < 0x240
        THROW_EXCEPTION("This function requires OpenCV > 2.4.0")
#else

        using namespace cv;

        vector<KeyPoint> cv_feats;  // OpenCV keypoint output vector
        Mat cv_descs;  // OpenCV descriptor output

        const bool use_precomputed_feats = feats.size() > 0;

        if (use_precomputed_feats)
        {
                cv_feats.resize(feats.size());
                for (size_t k = 0; k < cv_feats.size(); ++k)
                {
                        cv_feats[k].pt.x = feats[k]->x;
                        cv_feats[k].pt.y = feats[k]->y;
                }
        }

        // Make sure we operate on a gray-scale version of the image:
        const CImage inImg_gray(inImg, FAST_REF_OR_CONVERT_TO_GRAY);
        const Mat cvImg = cv::cvarrToMat(inImg_gray.getAs<IplImage>());

// The detector and descriptor
#if MRPT_OPENCV_VERSION_NUM < 0x300
        Ptr<Feature2D> orb = Algorithm::create<Feature2D>("Feature2D.ORB");
        orb->operator()(cvImg, Mat(), cv_feats, cv_descs, use_precomputed_feats);
#else
        const size_t n_feats_2_extract =
                nDesiredFeatures == 0 ? 1000 : 3 * nDesiredFeatures;
        Ptr<cv::ORB> orb = cv::ORB::create(
                n_feats_2_extract, options.ORBOptions.scale_factor,
                options.ORBOptions.n_levels);
        orb->detectAndCompute(
                cvImg, Mat(), cv_feats, cv_descs, use_precomputed_feats);
#endif

        const size_t n_feats = cv_feats.size();

        // if we had input features, just convert cv_feats to CFeatures and return
        const unsigned int patch_size_2 = options.patchSize / 2;
        unsigned int f_id = init_ID;
        if (use_precomputed_feats)
        {
                for (size_t k = 0; k < n_feats; ++k)
                {
                        feats[k]->descriptors.ORB.resize(cv_descs.cols);
                        for (int m = 0; m < cv_descs.cols; ++m)
                                feats[k]->descriptors.ORB[m] =
                                        cv_descs.at<uchar>(k, m);  // to do: memcopy

                        /*
                        feats[k].response       = cv_feats[k].response;
                        feats[k].scale          = cv_feats[k].size;
                        feats[k].angle          = cv_feats[k].orientation;
                        feats[k].ID                     = f_id++;
                        */
                        feats[k]->type = featORB;

                        if (options.ORBOptions.extract_patch && options.patchSize > 0)
                        {
                                inImg.extract_patch(
                                        feats[k]->patch, round(feats[k]->x) - patch_size_2,
                                        round(feats[k]->y) - patch_size_2, options.patchSize,
                                        options.patchSize);
                        }
                }
                return;
        }

        //  1) Sort the fearues by "response": It's ~100 times faster to sort a list
        //  of
        //      indices "sorted_indices" than sorting directly the actual list of
        //      features "cv_feats"
        std::vector<size_t> sorted_indices(n_feats);
        for (size_t i = 0; i < n_feats; i++) sorted_indices[i] = i;
        std::sort(
                sorted_indices.begin(), sorted_indices.end(),
                KeypointResponseSorter<vector<KeyPoint>>(cv_feats));

        //  2) Filter by "min-distance" (in options.ORBOptions.min_distance)
        //  3) Convert to MRPT CFeatureList format.
        // Steps 2 & 3 are done together in the while() below.
        // The "min-distance" filter is done by means of a 2D binary matrix where
        // each cell is marked when one
        // feature falls within it. This is not exactly the same than a pure
        // "min-distance" but is pretty close
        // and for large numbers of features is much faster than brute force search
        // of kd-trees.
        // (An intermediate approach would be the creation of a mask image updated
        // for each accepted feature, etc.)

        const bool do_filter_min_dist = options.ORBOptions.min_distance > 1;
        const unsigned int occupied_grid_cell_size =
                options.ORBOptions.min_distance / 2.0;
        const float occupied_grid_cell_size_inv = 1.0f / occupied_grid_cell_size;

        unsigned int grid_lx =
                !do_filter_min_dist
                        ? 1
                        : (unsigned int)(1 + inImg.getWidth() * occupied_grid_cell_size_inv);
        unsigned int grid_ly =
                !do_filter_min_dist
                        ? 1
                        : (unsigned int)(1 + inImg.getHeight() * occupied_grid_cell_size_inv);

        mrpt::math::CMatrixBool occupied_sections(
                grid_lx, grid_ly);  // See the comments above for an explanation.
        occupied_sections.fillAll(false);

        const size_t n_max_feats = nDesiredFeatures > 0
                                                                   ? std::min(size_t(nDesiredFeatures), n_feats)
                                                                   : n_feats;

        if (!options.addNewFeatures) feats.clear();
        // feats.reserve( feats.size() + n_max_feats );

        const size_t imgH = inImg.getHeight();
        const size_t imgW = inImg.getWidth();
        size_t k = 0;
        size_t c_feats = 0;
        while (c_feats < n_max_feats && k < n_feats)
        {
                const size_t idx = sorted_indices[k++];
                const KeyPoint& kp = cv_feats[idx];
                if (options.ORBOptions.extract_patch && options.patchSize > 0)
                {
                        // check image boundaries for extracting the patch
                        const int xBorderInf = (int)floor(kp.pt.x - patch_size_2);
                        const int xBorderSup = (int)floor(kp.pt.x + patch_size_2);
                        const int yBorderInf = (int)floor(kp.pt.y - patch_size_2);
                        const int yBorderSup = (int)floor(kp.pt.y + patch_size_2);

                        if (!(xBorderSup < (int)imgW && xBorderInf > 0 &&
                                  yBorderSup < (int)imgH && yBorderInf > 0))
                                continue;  // nope, skip.
                }

                if (do_filter_min_dist)
                {
                        // Check the min-distance:
                        const size_t section_idx_x =
                                size_t(kp.pt.x * occupied_grid_cell_size_inv);
                        const size_t section_idx_y =
                                size_t(kp.pt.y * occupied_grid_cell_size_inv);

                        if (occupied_sections(section_idx_x, section_idx_y))
                                continue;  // Already occupied! skip.

                        // Mark section as occupied
                        occupied_sections.set_unsafe(section_idx_x, section_idx_y, true);
                        if (section_idx_x > 0)
                                occupied_sections.set_unsafe(
                                        section_idx_x - 1, section_idx_y, true);
                        if (section_idx_y > 0)
                                occupied_sections.set_unsafe(
                                        section_idx_x, section_idx_y - 1, true);
                        if (section_idx_x < grid_lx - 1)
                                occupied_sections.set_unsafe(
                                        section_idx_x + 1, section_idx_y, true);
                        if (section_idx_y < grid_ly - 1)
                                occupied_sections.set_unsafe(
                                        section_idx_x, section_idx_y + 1, true);
                }

                // All tests passed: add new feature:
                CFeature::Ptr ft = mrpt::make_aligned_shared<CFeature>();
                ft->type = featORB;
                ft->ID = f_id++;
                ft->x = kp.pt.x;
                ft->y = kp.pt.y;
                ft->response = kp.response;
                ft->orientation = kp.angle;
                ft->scale = kp.octave;
                ft->patchSize = 0;

                // descriptor
                ft->descriptors.ORB.resize(cv_descs.cols);
                for (int m = 0; m < cv_descs.cols; ++m)
                        ft->descriptors.ORB[m] = cv_descs.at<uchar>(idx, m);

                if (options.ORBOptions.extract_patch && options.patchSize > 0)
                {
                        ft->patchSize = options.patchSize;  // The size of the feature patch

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

                feats.push_back(ft);
                c_feats++;
        }
#endif
#endif
        MRPT_END
}

void CFeatureExtraction::internal_computeORBDescriptors(
        const CImage& in_img, CFeatureList& in_features) const
{
#if MRPT_HAS_OPENCV
#if MRPT_OPENCV_VERSION_NUM < 0x240
        THROW_EXCEPTION("This function requires OpenCV > 2.4.0")
#else
        using namespace cv;

        const size_t n_feats = in_features.size();
        const CImage inImg_gray(in_img, FAST_REF_OR_CONVERT_TO_GRAY);

        // convert from CFeatureList to vector<KeyPoint>
        vector<KeyPoint> cv_feats(n_feats);
        for (size_t k = 0; k < n_feats; ++k)
        {
                KeyPoint& kp = cv_feats[k];
                kp.pt.x = in_features[k]->x;
                kp.pt.y = in_features[k]->y;
                kp.angle = in_features[k]->orientation;
                kp.size = in_features[k]->scale;
        }  // end-for

        Mat cvImg(cv::cvarrToMat(inImg_gray.getAs<IplImage>()));
        Mat cv_descs;

#if MRPT_OPENCV_VERSION_NUM < 0x300
        Ptr<Feature2D> orb = Algorithm::create<Feature2D>("Feature2D.ORB");
        orb->operator()(
                cvImg, Mat(), cv_feats, cv_descs, true /* use_precomputed_feats */);
#else
        Ptr<cv::ORB> orb = cv::ORB::create(
                n_feats, options.ORBOptions.scale_factor, options.ORBOptions.n_levels);
        orb->detectAndCompute(
                cvImg, Mat(), cv_feats, cv_descs, true /* use_precomputed_feats */);
#endif

        // add descriptor to CFeatureList
        for (size_t k = 0; k < n_feats; ++k)
        {
                in_features[k]->descriptors.ORB.resize(cv_descs.cols);
                for (int i = 0; i < cv_descs.cols; ++i)
                        in_features[k]->descriptors.ORB[i] = cv_descs.at<uchar>(k, i);

        }  // end-for
#endif
#endif

}  // end-internal_computeORBImageDescriptors