features.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 BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the authors at <alessandro.fornasier@ieee.org>
 
#ifndef FEATURES_HPP
#define FEATURES_HPP
 
#include <opencv2/opencv.hpp>
 
#include "types/fptypes.hpp"
 
namespace msceqf
{
using FeaturesCoordinates = std::vector<cv::Point2f>;  
 
struct Features
{
  using FeatureIds = std::vector<uint>;  
 
  inline bool empty() const noexcept { return uvs_.empty(); }
 
  inline size_t size() const noexcept
  {
    assert(distorted_uvs_.size() == uvs_.size());
    assert(distorted_uvs_.size() == normalized_uvs_.size());
    assert(distorted_uvs_.size() == ids_.size());
    return distorted_uvs_.size();
  }
 
  void removeInvalid(std::vector<bool>& invalid)
  {
    assert(invalid.size() == distorted_uvs_.size());
    assert(distorted_uvs_.size() == uvs_.size());
    assert(distorted_uvs_.size() == normalized_uvs_.size());
    assert(distorted_uvs_.size() == ids_.size());
 
    size_t i = 0;
    size_t j = 0;
 
    while (i < invalid.size())
    {
      if (!invalid[i])
      {
        distorted_uvs_[j] = distorted_uvs_[i];
        uvs_[j] = uvs_[i];
        normalized_uvs_[j] = normalized_uvs_[i];
        ids_[j] = ids_[i];
        ++j;
      }
      ++i;
    }
 
    distorted_uvs_.resize(j);
    uvs_.resize(j);
    normalized_uvs_.resize(j);
    ids_.resize(j);
  }
 
  FeaturesCoordinates distorted_uvs_;   
  FeaturesCoordinates uvs_;             
  FeaturesCoordinates normalized_uvs_;  
  FeatureIds ids_;                      
};
 
}  // namespace msceqf
 
#endif  // FEATURES_HPP