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};         
    double threshold {20.0};                  
    double px_error{1e-9};                    
    double min_feat_dist {1.0};               
    bool down_sample {false};                 
    double down_sample_height {480.0};        
    double down_sample_width {640.0};         
    bool is_cam_extrinsic{false};             
  } Parameters;
 
  explicit FeatureTracker(FeatureTracker::Parameters params);
 
  std::vector<cv::KeyPoint> GridFeatures(
    std::vector<cv::KeyPoint> key_points,
    unsigned int rows,
    unsigned int cols);
 
  void Track(double time, int frame_id, const cv::Mat & img_in, cv::Mat & img_out);
 
 
  void RatioTest(std::vector<std::vector<cv::DMatch>> & matches);
 
  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
  );
 
  void DistanceTest(
    std::vector<cv::DMatch> & matches_in,
    std::vector<cv::KeyPoint> & curr_key_points,
    std::vector<cv::DMatch> & matches_out
  );
 
protected:
  MsckfUpdater m_msckf_updater;  
 
private:
  cv::Ptr<cv::FeatureDetector> InitFeatureDetector(Detector detector, double threshold);
  cv::Ptr<cv::DescriptorExtractor> InitDescriptorExtractor(Descriptor extractor, double threshold);
  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;
 
  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;
 
  unsigned int GenerateFeatureID();
 
  double m_px_error;
  bool m_down_sample;
  double m_down_sample_height;
  double m_down_sample_width;
  double m_knn_ratio{0.7};
};
 
#endif  // TRACKERS__FEATURE_TRACKER_HPP_