tracker.hpp

// Copyright (C) 2023 Alessandro Fornasier.
// Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the Apache License, Version 2.0
// (the "License"); you may not use this file except in compliance with the
// License. You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations
// under the License.
//
// You can contact the authors at <alessandro.fornasier@ieee.org>
 
#ifndef TRACKER_HPP
#define TRACKER_HPP
 
#include <atomic>
 
#include "sensors/sensor_data.hpp"
#include "types/fptypes.hpp"
#include "vision/camera.hpp"
#include "vision/features.hpp"
#include "vision/track.hpp"
 
namespace msceqf
{
class Tracker
{
 public:
  using Keypoints = std::vector<cv::KeyPoint>;    
  using TimedFeatures = std::pair<fp, Features>;  

  Tracker(const TrackerOptions& opts, const Vector4& intrinsics);

  void processCamera(Camera& cam);

  const TimedFeatures& currentFeatures() const;

  const PinholeCameraUniquePtr& cam() const;
 
 private:
  void track(Camera& cam);

  void detect(std::vector<cv::Mat>& pyramids, cv::Mat& mask, Features& features);

  void matchKLT(std::vector<uchar>& mask);

  void ransac(std::vector<uchar>& mask);

  void extractCellKeypoints(const uint& idx, const cv::Mat& cell, const cv::Mat& mask, Keypoints& cell_kpts);

  void maskGivenFeatures(cv::Mat& mask, const FeaturesCoordinates& points);
 
  TrackerOptions opts_;  
 
  PinholeCameraUniquePtr cam_;       
  cv::Ptr<cv::Feature2D> detector_;  
 
  std::map<uint, std::atomic<uint>> max_kpts_per_cell_;  
  uint id_;                                              
 
  cv::Mat feature_mask_;  
 
  std::vector<cv::Mat> previous_pyramids_;  
  TimedFeatures previous_features_;         
 
  std::vector<cv::Mat> current_pyramids_;  
  TimedFeatures current_features_;         
 
  cv::Size win_;  
 
  static constexpr std::array<uint, 4> ratio_ = {10, 6, 3, 1};  
};
 
}  // namespace msceqf
 
#endif  // TRACKER_HPP