Program Listing for File feature-tracker-gyro.h

↰ Return to documentation for file (aslam_cv2/aslam_cv_tracker/include/aslam/tracker/feature-tracker-gyro.h)

#ifndef ASLAM_GYRO_TRACKER_H_
#define ASLAM_GYRO_TRACKER_H_

#include <array>
#include <deque>
#include <memory>
#include <vector>

#include <aslam/common/macros.h>
#include <Eigen/Dense>
#include <glog/logging.h>
#include <opencv2/features2d/features2d.hpp>

#include "aslam/tracker/feature-tracker.h"

namespace aslam {
class VisualFrame;
class Camera;

struct GyroTrackerSettings {
  GyroTrackerSettings();

  double lk_max_num_candidates_ratio_kp1;
  size_t lk_max_status_track_length;

  // calcOpticalFlowPyrLK parameters:
  cv::TermCriteria lk_termination_criteria;
  cv::Size lk_window_size;
  int lk_max_pyramid_levels;
  int lk_operation_flag;
  double lk_min_eigenvalue_threshold;

  // Keypoint uncertainty.
  static constexpr double kKeypointUncertaintyPx = 0.8;
};


class GyroTracker : public FeatureTracker{
 public:
  ASLAM_DISALLOW_EVIL_CONSTRUCTORS(GyroTracker);
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 public:
  explicit GyroTracker(const Camera& camera,
                       const size_t min_distance_to_image_border,
                       const cv::Ptr<cv::DescriptorExtractor>& extractor_ptr);
  virtual ~GyroTracker() {}

  virtual void track(const Quaternion& q_Ckp1_Ck,
                     const VisualFrame& frame_k,
                     VisualFrame* frame_kp1,
                     FrameToFrameMatchesWithScore* matches_kp1_k) override;

 private:
  enum class FeatureStatus {
    kDetected,
    kLkTracked
  };

  // first: index_k, second: index_km1.
  typedef std::pair<int, int> TrackedMatch;
  typedef std::vector<FeatureStatus> FrameFeatureStatus;
  typedef std::vector<size_t> FrameStatusTrackLength;
  typedef Eigen::VectorXi TrackIds;

  virtual void lkTracking(
      const Eigen::Matrix2Xd& predicted_keypoint_positions_kp1,
      const std::vector<unsigned char>& prediction_success,
      const std::vector<int>& lk_candidate_indices_k,
      const VisualFrame& frame_k,
      VisualFrame* frame_kp1,
      FrameToFrameMatchesWithScore* matches_kp1_k);

  virtual void computeLKCandidates(
      const FrameToFrameMatchesWithScore& matches_kp1_k,
      const FrameStatusTrackLength& status_track_length_k,
      const VisualFrame& frame_k,
      const VisualFrame& frame_kp1,
      std::vector<int>* lk_candidate_indices_k) const;

  virtual void computeTrackedMatches(
      std::vector<TrackedMatch>* tracked_matches) const;

  virtual void computeUnmatchedIndicesOfFrameK(
      const FrameToFrameMatchesWithScore& matches_kp1_k,
      std::vector<int>* unmatched_indices_k) const;

  virtual void computeStatusTrackLengthOfFrameK(
      const std::vector<TrackedMatch>& tracked_matches,
      FrameStatusTrackLength* status_track_length_k);

  virtual void initializeFeatureStatusDeque();

  virtual void updateFeatureStatusDeque(
      const FrameFeatureStatus& frame_feature_status_kp1);

  virtual void updateTrackIdDeque(
      const VisualFrame& new_frame_k);

  template <typename Type>
  void eraseVectorElementsByIndex(
          const std::unordered_set<size_t>& indices_to_erase,
          std::vector<Type>* vec) const;

  const aslam::Camera& camera_;
  const size_t kMinDistanceToImageBorderPx;
  const cv::Ptr<cv::DescriptorExtractor> extractor_;
  bool initialized_;
  // Store track IDs of frame k and (k-1) in that order.
  std::deque<TrackIds> track_ids_k_km1_;
  std::deque<FrameFeatureStatus> feature_status_k_km1_;
  FrameStatusTrackLength status_track_length_km1_;

  const GyroTrackerSettings settings_;
};

template <typename Type>
void GyroTracker::eraseVectorElementsByIndex(
        const std::unordered_set<size_t>& indices_to_erase,
        std::vector<Type>* vec) const {
  CHECK_NOTNULL(vec);
    std::vector<bool> erase_index(vec->size(), false);
    for (const size_t i: indices_to_erase) {
        erase_index[i] = true;
    }
    std::vector<bool>::const_iterator it_to_erase = erase_index.begin();
    typename std::vector<Type>::iterator it_erase_from = std::remove_if(
        vec->begin(), vec->end(),
        [&it_to_erase](const Type& /*whatever*/) -> bool {
          return *it_to_erase++ == true;
        }
    );
    vec->erase(it_erase_from, vec->end());
    vec->shrink_to_fit();
}

} // namespace aslam

#endif  // ASLAM_GYRO_TRACKER_H_