tracking.h

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 |
   +------------------------------------------------------------------------+ */

#ifndef mrpt_vision_tracking_H
#define mrpt_vision_tracking_H

#include <mrpt/vision/types.h>

#include <mrpt/vision/CFeature.h>
#include <mrpt/vision/TSimpleFeature.h>
#include <mrpt/utils/CImage.h>
#include <mrpt/utils/CTimeLogger.h>
#include <mrpt/utils/TParameters.h>

#include <mrpt/utils/metaprogramming.h>

#include <memory>  // for unique_ptr

namespace mrpt
{
namespace vision
{
/** \addtogroup vision_tracking Feature detection and tracking
  *  \ingroup mrpt_vision_grp
  *   @{  */

/** A virtual interface for all feature trackers, implementing the part of
 * feature tracking that is common to any specific tracker implementation.
  *   This class provides a quite robust tracking of features, avoiding as many
 * outliers as possible but not all of them:
  *    more robust tracking would require application-specific information and
 * could be done in a number of very different approaches,
  *    so this class will not try to do any kind of RANSAC or any other advanced
 * outlier rejection; instead, it should
  *    be done by the users or the classes that employ this class.
  *
  *   The basic usage of this class is as follows:
  *    \code
  *       CFeatureTracker_KL    tracker;  // Note: CFeatureTracker_KL is the
 * most robust implementation for now.
  *       tracker.extra_params["add_new_features"] = 1;  // Enable detection of
 * new features, not only tracking
  *       tracker.extra_params[...] = ...
  *       // ....
  *       CFeatureList  theFeats;  // The list of features
  *       mrpt::utils::CImage  previous_img, current_img;
  *
  *       while (true) {
  *           current_img = ... // Grab new image.
  *           if ( previous_img_is_ok )
  *               tracker.trackFeatures(previous_img, current_img, theFeats);
  *           previous_img = current_img;
  *       }
  *    \endcode
  *
  *  Below follows the list of optional parameters for "extra_params" which can
 * be set
  *  and will be understood by this base class for any specific tracker
 * implementation.
  *  Note that all parameters are double's, but boolean flags are emulated by
 * the values 0.0 (false) and 1.0 (true).
  *
  *  List of parameters:
  * <table border="1" >
  *   <tr><td align="center" > <b>Parameter name</b>  </td>  <td align="center"
 * > <b>Default value</b> </td> <td align="center" > <b>Comments</b> </td> </tr>
  *   <tr><td align="center" > add_new_features  </td>  <td align="center" > 0
 * </td>
  *      <td> If set to "1", the class will not only track existing features,
 * but will also perform (after doing the actual tracking) an efficient
  *            search for new features with the FAST detector, and will add them
 * to the passed "CFeatureList" if they fulfill a set of restrictions,
  *            as stablished by the other parameters (see
 * <i>add_new_feat_min_separation</i>,<i>add_new_feat_max_features</i>,<i>minimum_KLT_response_to_add</i>).
  *        </td> </tr>
  *   <tr><td align="center" > add_new_feat_min_separation  </td>  <td
 * align="center" > 15 </td>
  *      <td> If <i>add_new_features</i>==1,  this is the minimum separation (in
 * pixels) to any other (old, or new) feature for it
  *             being considered a candidate to be added.
  *         </td> </tr>
  *   <tr><td align="center" > desired_num_features_adapt  </td>  <td
 * align="center" > (img_width*img_height)/512 </td>
  *      <td> If <i>add_new_features</i>==1, the threshold of the FAST(ER)
 * feature detector is dynamically adapted such as the number of
  *        raw FAST keypoints is around this number. This number should be much
 * higher than the real desired numbre of features, since this
  *        one includes many features concentrated in space which are later
 * discarded for the minimum distance.
  *         </td> </tr>
  *   <tr><td align="center" > desired_num_features </td>  <td align="center" >
 * 100 </td>
  *      <td> If <i>add_new_features</i>==1, the target number of the patch
 * associated to each feature will be updated with every N'th frame. </td> </tr>
  *   <tr><td align="center" > add_new_feat_patch_size  </td>  <td
 * align="center" > 11 </td>
  *      <td> If <i>add_new_features</i>==1,  for each new added feature, this
 * is the size of the patch to be extracted around the keypoint (set to 0 if
 * patches are not required at all).
  *          </td> </tr>
  *   <tr><td align="center" > minimum_KLT_response_to_add  </td>  <td
 * align="center" > 10 </td>
  *      <td> If <i>add_new_features</i>==1, this sets the minimum KLT response
 * of candidate FAST features to be added in each frame, if they also fulfil the
 * other restrictions (e.g. min.distance).
  *         </td> </tr>
  *   <tr><td align="center" > check_KLT_response_every  </td>  <td
 * align="center" > 0 </td>
  *      <td> If >0, it will compute the KLT response at each feature point
 * every <i>N</i> frames
  *            and those below <i>minimum_KLT_response</i> will be marked as
 * "lost" in their "track_status" field.
  *        </td> </tr>
  *   <tr><td align="center" > minimum_KLT_response  </td>  <td align="center" >
 * 5 </td>
  *      <td> See explanation of <i>check_KLT_response_every</i>.
  *        </td> </tr>
  *   <tr><td align="center" > KLT_response_half_win  </td>  <td align="center"
 * > 4 </td>
  *      <td> When computing the KLT response of features (see
 * <i>minimum_KLT_response</i> and <i>minimum_KLT_response_to_add</i>),
  *            the window centered at the point for its estimation will be of
 * size (2*W+1)x(2*W+1), with <i>W</i> being this parameter value.
  *       </td> </tr>
  *   <tr><td align="center" > update_patches_every  </td>  <td align="center" >
 * 0 </td>
  *      <td> If !=0, the patch associated to each feature will be updated with
 * every N'th frame. </td> </tr>
  *   <tr><td align="center" > remove_lost_features  </td>  <td align="center" >
 * 0 </td>
  *      <td> If !=0, out-of-bound features or those lost while tracking, will
 * be automatically removed from the list of features.
  *            Otherwise, the user will have to manually remove them by checking
 * the track_status field. </td> </tr>
  * </table>
  *
  *  This class also offers a time profiler, disabled by default (see
 * getProfiler and enableTimeLogger).
  *
  * \sa CFeatureTracker_KL, the example application "track-video-features".
  */
struct CGenericFeatureTracker
{
        /** Optional list of extra parameters to the algorithm. */
        mrpt::utils::TParametersDouble extra_params;

        /** Default ctor */
        inline CGenericFeatureTracker()
                : m_timlog(false),
                  m_update_patches_counter(0),
                  m_check_KLT_counter(0),
                  m_detector_adaptive_thres(10)
        {
        }
        /** Ctor with extra parameters */
        inline CGenericFeatureTracker(mrpt::utils::TParametersDouble extraParams)
                : extra_params(extraParams),
                  m_timlog(false),
                  m_update_patches_counter(0),
                  m_check_KLT_counter(0),
                  m_detector_adaptive_thres(10)
        {
        }
        /** Dtor */
        virtual ~CGenericFeatureTracker() {}
        /** Perform feature tracking from "old_img" to "new_img", with a (possibly
          *empty) list of previously tracked features "inout_featureList".
          *  This is a list of parameters (in "extraParams") accepted by ALL
          *implementations of feature tracker (see each derived class for more
          *specific parameters).
          *             - "add_new_features" (Default=0). If set to "1", new features will
          *be
          *also added to the existing ones in areas of the image poor of features.
          * This method does:
          *    - Convert old and new images to grayscale, if they're in color.
          *    - Call the pure virtual "trackFeatures_impl" method.
          *    - Implement the optional detection of new features if
          *"add_new_features"!=0.
          */
        void trackFeatures(
                const mrpt::utils::CImage& old_img, const mrpt::utils::CImage& new_img,
                TSimpleFeatureList& inout_featureList);

        /** overload with subpixel precision */
        void trackFeatures(
                const mrpt::utils::CImage& old_img, const mrpt::utils::CImage& new_img,
                TSimpleFeaturefList& inout_featureList);

        /** overload This overload version uses the old (and much slower)
         * CFeatureList  */
        void trackFeatures(
                const mrpt::utils::CImage& old_img, const mrpt::utils::CImage& new_img,
                CFeatureList& inout_featureList);

        /** A wrapper around the basic trackFeatures() method, but keeping the
         * original list of features unmodified and returns the tracked ones in a
         * new list. */
        inline void trackFeaturesNewList(
                const mrpt::utils::CImage& old_img, const mrpt::utils::CImage& new_img,
                const vision::CFeatureList& in_featureList,
                vision::CFeatureList& out_featureList)
        {
                out_featureList = in_featureList;
                std::for_each(
                        out_featureList.begin(), out_featureList.end(),
                        mrpt::utils::metaprogramming::ObjectMakeUnique());
                this->trackFeatures(old_img, new_img, out_featureList);
        }

        /** Returns a read-only reference to the internal time logger */
        inline const mrpt::utils::CTimeLogger& getProfiler() const
        {
                return m_timlog;
        }
        /** Returns a reference to the internal time logger */
        inline mrpt::utils::CTimeLogger& getProfiler() { return m_timlog; }
        /** Returns a read-only reference to the internal time logger */
        inline void enableTimeLogger(bool enable = true)
        {
                m_timlog.enable(enable);
        }

        /** Returns the current adaptive threshold used by the FAST(ER) detector to
         * find out new features in empty areas */
        inline int getDetectorAdaptiveThreshold() const
        {
                return m_detector_adaptive_thres;
        }

        struct TExtraOutputInfo
        {
                /** In the new_img with the last adaptive threshold */
                size_t raw_FAST_feats_detected;
                /** The number of features which were deleted due to OOB, bad tracking,
                 * etc... (only if "remove_lost_features" is enabled) */
                size_t num_deleted_feats;
        };

        /** Updated with each call to trackFeatures() */
        TExtraOutputInfo last_execution_extra_info;

   protected:
        /** The tracking method implementation, to be implemented in children
         * classes. */
        virtual void trackFeatures_impl(
                const mrpt::utils::CImage& old_img, const mrpt::utils::CImage& new_img,
                TSimpleFeaturefList& inout_featureList);

        /** The tracking method implementation, to be implemented in children
         * classes. */
        virtual void trackFeatures_impl(
                const mrpt::utils::CImage& old_img, const mrpt::utils::CImage& new_img,
                TSimpleFeatureList& inout_featureList) = 0;

        /** This version falls back to the version with TSimpleFeatureList if the
         * derived class does not implement it. */
        virtual void trackFeatures_impl(
                const mrpt::utils::CImage& old_img, const mrpt::utils::CImage& new_img,
                CFeatureList& inout_featureList) = 0;

        /** the internal time logger, disabled by default. */
        mrpt::utils::CTimeLogger m_timlog;

        /** This field is clared by \a trackFeatures() before calling \a
         * trackFeatures_impl(), and
          *   can be filled out with newly defected FAST(ER) features in the latter.
          * If it's not the case, feats will be computed anyway if the user enabled
         * the "add_new_features" option.
          */
        mrpt::vision::TSimpleFeatureList m_newly_detected_feats;

        /** Adapts the threshold \a m_detector_adaptive_thres according to the real
         * and desired number of features just detected */
        void updateAdaptiveNewFeatsThreshold(
                const size_t nNewlyDetectedFeats, const size_t desired_num_features);

   private:
        /** for use when "update_patches_every">=1 */
        size_t m_update_patches_counter;
        /** For use when "check_KLT_response_every">=1 */
        size_t m_check_KLT_counter;
        /** For use in "add_new_features" == true */
        int m_detector_adaptive_thres;

        template <typename FEATLIST>
        void internal_trackFeatures(
                const mrpt::utils::CImage& old_img, const mrpt::utils::CImage& new_img,
                FEATLIST& inout_featureList);
};

typedef std::unique_ptr<CGenericFeatureTracker> CGenericFeatureTrackerAutoPtr;

/** Track a set of features from old_img -> new_img using sparse optimal flow
  *(classic KL method).
  *
  *  See CGenericFeatureTracker for a more detailed explanation on how to use
  *this class.
  *
  *   List of additional parameters in "extra_params" (apart from those in
  *CGenericFeatureTracker) accepted by this class:
  *             - "window_width"  (Default=15)
  *             - "window_height" (Default=15)
  *             - "LK_levels" (Default=3) Number of pyramids to build for LK tracking
  *(this
  *parameter only has effects when tracking with CImage's, not with
  *CImagePyramid's).
  *             - "LK_max_iters" (Default=10) Max. number of iterations in LK tracking.
  *             - "LK_epsilon" (Default=0.1) Minimum epsilon step in interations of
  *LK_tracking.
  *             - "LK_max_tracking_error" (Default=150.0) The maximum "tracking error"
  *of
  *LK tracking such as a feature is marked as "lost".
  *
  *  \sa OpenCV's method cvCalcOpticalFlowPyrLK
  */
struct CFeatureTracker_KL : public CGenericFeatureTracker
{
        /** Default ctor */
        inline CFeatureTracker_KL() {}
        /** Ctor with extra parameters */
        inline CFeatureTracker_KL(mrpt::utils::TParametersDouble extraParams)
                : CGenericFeatureTracker(extraParams)
        {
        }

   protected:
        virtual void trackFeatures_impl(
                const mrpt::utils::CImage& old_img, const mrpt::utils::CImage& new_img,
                vision::CFeatureList& inout_featureList) override;
        virtual void trackFeatures_impl(
                const mrpt::utils::CImage& old_img, const mrpt::utils::CImage& new_img,
                TSimpleFeatureList& inout_featureList) override;
        virtual void trackFeatures_impl(
                const mrpt::utils::CImage& old_img, const mrpt::utils::CImage& new_img,
                TSimpleFeaturefList& inout_featureList) override;

   private:
        template <typename FEATLIST>
        void trackFeatures_impl_templ(
                const mrpt::utils::CImage& old_img, const mrpt::utils::CImage& new_img,
                FEATLIST& inout_featureList);
};

/** Search for correspondences which are not in the same row and deletes them
  * ...
  */
void checkTrackedFeatures(
        CFeatureList& leftList, CFeatureList& rightList,
        vision::TMatchingOptions options);

/** Filter bad correspondences by distance
  * ...
  */
void filterBadCorrsByDistance(
        mrpt::utils::TMatchingPairList& list,  // The list of correspondences
        unsigned int numberOfSigmas);  // Threshold

/**  @}  */  // end of grouping
}
}

#endif