feature_tracker.hpp

// Copyright 2023 Jacob Hartzer
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.
 
#ifndef TRACKERS__FEATURE_TRACKER_HPP_
#define TRACKERS__FEATURE_TRACKER_HPP_
 
#include <map>
#include <memory>
#include <string>
#include <vector>
 
#include <opencv2/features2d.hpp>
#include <opencv2/opencv.hpp>
 
#include "ekf/ekf.hpp"
#include "ekf/types.hpp"
#include "ekf/update/msckf_updater.hpp"
#include "infrastructure/debug_logger.hpp"
#include "trackers/tracker.hpp"
 
enum class Detector
{
  BRISK,
  FAST,
  GFTT,
  MSER,
  ORB,
  SIFT,
};
 
enum class Descriptor
{
  ORB,
  SIFT
};
 
enum class Matcher
{
  BRUTE_FORCE,
  FLANN
};
 
class FeatureTracker : public Tracker
{
public:
  typedef struct Parameters : public Tracker::Parameters
  {
    Detector detector {Detector::FAST};       
    Descriptor descriptor {Descriptor::ORB};  
    Matcher matcher {Matcher::FLANN};         
    int threshold {20};                       
    double px_error{1e-9};                    
    double min_feat_dist {1.0};               
    bool down_sample {false};                 
    int down_sample_height {480};             
    int down_sample_width {640};              
    bool is_cam_extrinsic{false};             
  } Parameters;
 
  explicit FeatureTracker(FeatureTracker::Parameters params);
 
  static std::vector<cv::KeyPoint> GridFeatures(
    std::vector<cv::KeyPoint> & key_points,
    int rows,
    int cols
  );
 
  void Track(
    double time,
    unsigned int frame_id,
    const cv::Mat & img_in,
    cv::Mat & img_out
  );
 
  void RatioTest(std::vector<std::vector<cv::DMatch>> & matches) const;
 
  static void SymmetryTest(
    std::vector<std::vector<cv::DMatch>> & matches_forward,
    std::vector<std::vector<cv::DMatch>> & matches_backward,
    std::vector<cv::DMatch> & matches_out
  );
 
  void RANSAC(
    std::vector<cv::DMatch> & matches_in,
    std::vector<cv::KeyPoint> & curr_key_points,
    std::vector<cv::DMatch> & matches_out
  ) const;
 
  void DistanceTest(
    std::vector<cv::DMatch> & matches_in,
    std::vector<cv::KeyPoint> & curr_key_points,
    std::vector<cv::DMatch> & matches_out
  ) const;
 
protected:
  MsckfUpdater m_msckf_updater;  
 
private:
  static cv::Ptr<cv::FeatureDetector> InitFeatureDetector(
    Detector detector,
    int threshold);
  static cv::Ptr<cv::DescriptorExtractor> InitDescriptorExtractor(
    Descriptor extractor,
    int threshold);
  static cv::Ptr<cv::DescriptorMatcher> InitDescriptorMatcher(Matcher matcher);
 
  cv::Ptr<cv::FeatureDetector> m_feature_detector;
  cv::Ptr<cv::DescriptorExtractor> m_descriptor_extractor;
  cv::Ptr<cv::DescriptorMatcher> m_descriptor_matcher;
 
  unsigned int m_prev_frame_id;
  double m_prev_frame_time;
  cv::Mat m_prev_descriptors;
  std::vector<cv::KeyPoint> m_prev_key_points;
 
  std::map<unsigned int, std::vector<FeaturePoint>> m_feature_points_map;
 
  static int GenerateFeatureID();
 
  double m_px_error;
  bool m_down_sample;
  int m_down_sample_height;
  int m_down_sample_width;
  double m_knn_ratio{0.7};
};
 
#endif  // TRACKERS__FEATURE_TRACKER_HPP_