TSimpleFeature.h

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 |
   +------------------------------------------------------------------------+ */
#ifndef _mrpt_vision_TSimpleFeature_H
#define _mrpt_vision_TSimpleFeature_H
 
#include <mrpt/img/TPixelCoord.h>
#include <mrpt/core/round.h>
#include <mrpt/math/KDTreeCapable.h>
#include <mrpt/math/CMatrixTemplate.h>  // mrpt::math::CMatrixBool
#include <mrpt/math/CMatrixTemplateNumeric.h>
#include <mrpt/vision/types.h>
#include <mrpt/core/round.h>
#include <functional>
 
namespace mrpt
{
namespace vision
{
/** \addtogroup  mrptvision_features
        @{ */
 
/** A simple structure for representing one image feature (without descriptor
 * nor patch) - This is
 *  the template which allows you to select if pixels are represented as
 * integers or floats.
 *  \sa TSimpleFeature, TSimpleFeaturef
 */
template <typename PIXEL_COORD_TYPE>
struct TSimpleFeature_templ
{
        /** The type of \a pt */
        using pixel_coords_t = PIXEL_COORD_TYPE;
        /** The type of pt.x and pt.y */
        using pixel_coord_t = typename PIXEL_COORD_TYPE::pixel_coord_t;
 
        /** Coordinates in the image */
        pixel_coords_t pt;
        /** ID of the feature */
        TFeatureID ID;
        /** Status of the feature tracking process */
        TFeatureTrackStatus track_status;
        /** A measure of the "goodness" of the feature (typically, the KLT_response
         * value) */
        float response;
        /** The image octave the image was found in: 0=original image, 1=1/2 image,
         * 2=1/4 image, etc. */
        uint8_t octave;
        /** A field for any other flags needed by the user (this has not a
         * predefined meaning) */
        uint8_t user_flags;
 
        /** Constructor that only sets the pt.{x,y} values, leaving all other values
         * to *undefined values*. */
        template <typename COORD_TYPE>
        inline TSimpleFeature_templ(const COORD_TYPE x, const COORD_TYPE y)
                : pt(x, y)
        {
        }
 
        /** Default constructor, leaves all fields uninitialized */
        inline TSimpleFeature_templ() {}
        template <typename OTHER_TSIMPLEFEATURE>
        explicit TSimpleFeature_templ(const OTHER_TSIMPLEFEATURE& o)
                : pt(o.pt.x, o.pt.y),
                  ID(o.ID),
                  track_status(o.track_status),
                  response(o.response),
                  octave(o.octave),
                  user_flags(o.user_flags)
        {
        }
};
 
/** A simple structure for representing one image feature (without descriptor
 * nor patch).
 *  \sa TSimpleFeaturef, CFeature, TSimpleFeatureList
 */
using TSimpleFeature = TSimpleFeature_templ<mrpt::img::TPixelCoord>;
 
/** A version of  TSimpleFeature with subpixel precision */
using TSimpleFeaturef = TSimpleFeature_templ<mrpt::img::TPixelCoordf>;
 
template <typename FEATURE>
struct TSimpleFeatureTraits;
 
template <>
struct TSimpleFeatureTraits<TSimpleFeature>
{
        using coord_t = int;
 
        static inline coord_t f2coord(float f) { return mrpt::round(f); }
};
 
template <>
struct TSimpleFeatureTraits<TSimpleFeaturef>
{
        using coord_t = float;
 
        static inline coord_t f2coord(float f) { return f; }
};
 
/** A list of image features using the structure TSimpleFeature for each feature
 * - capable of KD-tree computations
 *  Users normally use directly the type TSimpleFeatureList &
 * TSimpleFeaturefList
 */
template <typename FEATURE>
struct TSimpleFeatureList_templ
{
   public:
        using TFeatureVector = std::vector<FEATURE>;
        using feature_t = FEATURE;
 
        /** @name Utilities
                @{ */
 
        /** Returns a const ref to the actual std::vector<> container */
        const TFeatureVector& getVector() const { return m_feats; }
        /** Returns the maximum ID of all features in the list, or 0 if it's empty
         */
        TFeatureID getMaxID() const
        {
                if (this->empty()) return 0;
                TFeatureID maxID = m_feats[0].ID;
                size_t N = m_feats.size() - 1;
                for (; N; --N) mrpt::keep_max(maxID, m_feats[N].ID);
                return maxID;
        }
 
        /** Returns a vector with a LUT of the first feature index per row, to
         * efficiently look for neighbors, etc.
         *  By default this vector is empty, so if a feature detector is used that
         * doesn't fill this out, it will remain empty and useless.
         *  \note FASTER detectors do fill this out. In general, a feature list
         * that dynamically changes will not use this LUT.
         */
        const std::vector<size_t>& getFirstIndexPerRowLUT() const
        {
                return m_first_index_per_row;
        }
        /// \overload
        std::vector<size_t>& getFirstIndexPerRowLUT()
        {
                return m_first_index_per_row;
        }
 
        /** Get a ref to the occupation matrix: this is a user-defined matrix, which
         * is not updated automatically by this class. */
        inline mrpt::math::CMatrixBool& getOccupiedSectionsMatrix()
        {
                return m_occupied_sections;
        }
        inline const mrpt::math::CMatrixBool& getOccupiedSectionsMatrix() const
        {
                return m_occupied_sections;
        }
 
        /** @} */
 
        /** @name Method and datatypes to emulate a STL container
                @{ */
        using iterator = typename TFeatureVector::iterator;
        using const_iterator = typename TFeatureVector::const_iterator;
 
        using reverse_iterator = typename TFeatureVector::reverse_iterator;
        using const_reverse_iterator =
                typename TFeatureVector::const_reverse_iterator;
 
        inline iterator begin() { return m_feats.begin(); }
        inline iterator end() { return m_feats.end(); }
        inline const_iterator begin() const { return m_feats.begin(); }
        inline const_iterator end() const { return m_feats.end(); }
        inline reverse_iterator rbegin() { return m_feats.rbegin(); }
        inline reverse_iterator rend() { return m_feats.rend(); }
        inline const_reverse_iterator rbegin() const { return m_feats.rbegin(); }
        inline const_reverse_iterator rend() const { return m_feats.rend(); }
        inline iterator erase(const iterator& it) { return m_feats.erase(it); }
        inline bool empty() const { return m_feats.empty(); }
        inline size_t size() const { return m_feats.size(); }
        inline void clear()
        {
                m_feats.clear();
                m_first_index_per_row.clear();
        }
        inline void resize(size_t N) { m_feats.resize(N); }
        inline void reserve(size_t N) { m_feats.reserve(N); }
        inline void push_back(const FEATURE& f) { m_feats.push_back(f); }
        inline void push_back_fast(const FEATURE& f) { m_feats.push_back(f); }
        inline void push_back_fast(const int x, const int y)
        {
                m_feats.push_back(FEATURE(x, y));
        }
 
        inline FEATURE& operator[](const unsigned int index)
        {
                return m_feats[index];
        }
        inline const FEATURE& operator[](const unsigned int index) const
        {
                return m_feats[index];
        }
 
        inline FEATURE& back() { return m_feats.back(); }
        inline const FEATURE& back() const { return m_feats.back(); }
        inline FEATURE& front() { return m_feats.front(); }
        inline const FEATURE& front() const { return m_feats.front(); }
        /** @} */
 
        /** @name getFeature*() methods for template-based access to feature list
                @{ */
        inline typename TSimpleFeatureTraits<FEATURE>::coord_t getFeatureX(
                size_t i) const
        {
                return m_feats[i].pt.x;
        }
        inline typename TSimpleFeatureTraits<FEATURE>::coord_t getFeatureY(
                size_t i) const
        {
                return m_feats[i].pt.y;
        }
        inline TFeatureID getFeatureID(size_t i) const { return m_feats[i].ID; }
        inline float getFeatureResponse(size_t i) const
        {
                return m_feats[i].response;
        }
        inline bool isPointFeature(size_t i) const
        {
                MRPT_UNUSED_PARAM(i);
                return true;
        }
        inline float getScale(size_t i) const
        {
                return static_cast<float>(1 << m_feats[i].octave);
        }
        inline TFeatureTrackStatus getTrackStatus(size_t i)
        {
                return m_feats[i].track_status;
        }
 
        inline void setFeatureX(
                size_t i, typename TSimpleFeatureTraits<FEATURE>::coord_t x)
        {
                m_feats[i].pt.x = x;
        }
        inline void setFeatureY(
                size_t i, typename TSimpleFeatureTraits<FEATURE>::coord_t y)
        {
                m_feats[i].pt.y = y;
        }
 
        inline void setFeatureXf(size_t i, float x)
        {
                m_feats[i].pt.x = TSimpleFeatureTraits<FEATURE>::f2coord(x);
        }
        inline void setFeatureYf(size_t i, float y)
        {
                m_feats[i].pt.y = TSimpleFeatureTraits<FEATURE>::f2coord(y);
        }
 
        inline void setFeatureID(size_t i, TFeatureID id) { m_feats[i]->ID = id; }
        inline void setFeatureResponse(size_t i, float r)
        {
                m_feats[i]->response = r;
        }
        inline void setScale(size_t i, float s)
        {
                m_feats[i]->octave = mrpt::round(std::log(s) / std::log(2));
        }
        inline void setTrackStatus(size_t i, TFeatureTrackStatus s)
        {
                m_feats[i].track_status = s;
        }
 
        inline void mark_as_outdated() const {}
        /** @} */
 
   private:
        /** The actual container with the list of features */
        TFeatureVector m_feats;
        /** A LUT of the first feature index per row, to efficiently look for
         * neighbors, etc. */
        std::vector<size_t> m_first_index_per_row;
        mrpt::math::CMatrixBool m_occupied_sections;
 
};  // end of class
 
/** A list of image features using the structure TSimpleFeature for each feature
 * - capable of KD-tree computations */
using TSimpleFeatureList = TSimpleFeatureList_templ<TSimpleFeature>;
 
/** A list of image features using the structure TSimpleFeaturef for each
 * feature - capable of KD-tree computations */
using TSimpleFeaturefList = TSimpleFeatureList_templ<TSimpleFeaturef>;
 
/** A helper struct to sort keypoints by their response: It can be used with
 *these types:
 *        - std::vector<cv::KeyPoint>
 *        - mrpt::vision::TSimpleFeatureList
 */
template <typename FEATURE_LIST>
struct KeypointResponseSorter
        : public std::function<bool(size_t, size_t)>
{
        const FEATURE_LIST& m_data;
        KeypointResponseSorter(const FEATURE_LIST& data) : m_data(data) {}
        bool operator()(size_t k1, size_t k2) const
        {
                return (m_data[k1].response > m_data[k2].response);
        }
};
 
/** Helper class: KD-tree search class for vector<KeyPoint>:
 *  Call mark_as_outdated() to force rebuilding the kd-tree after modifying the
 * linked feature list.
 *  \tparam FEAT Can be cv::KeyPoint or mrpt::vision::TSimpleFeature
 */
template <typename FEAT>
class CFeatureListKDTree
        : public mrpt::math::KDTreeCapable<CFeatureListKDTree<FEAT>>
{
   public:
        inline void mark_as_outdated()
        {
                mrpt::math::KDTreeCapable<
                        CFeatureListKDTree<FEAT>>::kdtree_mark_as_outdated();
        }
 
        const std::vector<FEAT>& m_data;
        CFeatureListKDTree(const std::vector<FEAT>& data) : m_data(data) {}
        /** @name Methods that MUST be implemented by children classes of
           KDTreeCapable
                @{ */
 
        /// Must return the number of data points
        inline size_t kdtree_get_point_count() const { return m_data.size(); }
        /// Returns the dim'th component of the idx'th point in the class:
        inline float kdtree_get_pt(const size_t idx, int dim) const
        {
                ASSERTDEB_(dim == 0 || dim == 1);
                if (dim == 0)
                        return m_data[idx].pt.x;
                else
                        return m_data[idx].pt.y;
        }
 
        /// Returns the distance between the vector "p1[0:size-1]" and the data
        /// point with index "idx_p2" stored in the class:
        inline float kdtree_distance(
                const float* p1, const size_t idx_p2, size_t size) const
        {
                MRPT_UNUSED_PARAM(size);  // in release mode
                ASSERTDEB_(size == 2);
 
                const float d0 = p1[0] - m_data[idx_p2].pt.x;
                const float d1 = p1[1] - m_data[idx_p2].pt.y;
                return d0 * d0 + d1 * d1;
        }
 
        // Optional bounding-box computation: return false to default to a standard
        // bbox computation loop.
        //   Return true if the BBOX was already computed by the class and returned
        //   in "bb" so it can be avoided to redo it again.
        //   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3
        //   for point clouds)
        template <typename BBOX>
        bool kdtree_get_bbox(BBOX& bb) const
        {
                MRPT_UNUSED_PARAM(bb);
                return false;
        }
 
        /** @} */
 
};  // end CFeatureListKDTree
 
/** @} */  // End of add to module: mrptvision_features
 
}  // namespace vision
 
}  // namespace mrpt
 
#endif