sensor_data.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 INPUT_HPP
#define INPUT_HPP
 
#include <opencv2/opencv.hpp>
 
#include "types/fptypes.hpp"
#include "vision/features.hpp"
 
namespace msceqf
{
struct Imu
{
  const Vector6 w() const { return (Vector6() << ang_, acc_).finished(); }
 
  const Matrix5 W() const
  {
    Matrix5 W = Matrix5::Zero();
    W.block<3, 3>(0, 0) = SO3::wedge(ang_);
    W.block<3, 1>(0, 3) = acc_;
    return W;
  }
 
  friend bool operator<(const Imu& lhs, const Imu& rhs) { return lhs.timestamp_ < rhs.timestamp_; }
 
  friend bool operator<(const Imu& lhs, const fp& timestamp) { return lhs.timestamp_ < timestamp; }
  friend bool operator<(const fp& timestamp, const Imu& rhs) { return timestamp < rhs.timestamp_; }
 
  friend std::ostream& operator<<(std::ostream& stream, Imu const& imu)
  {
    return stream << "(" << imu.timestamp_ << ", " << imu.ang_.transpose() << ", " << imu.acc_.transpose() << ")";
  }
 
  Vector3 ang_ = Vector3::Zero();  
  Vector3 acc_ = Vector3::Zero();  
  fp timestamp_ = -1;              
};
 
struct Camera
{
  friend bool operator<(const Camera& lhs, const Camera& rhs) { return lhs.timestamp_ < rhs.timestamp_; }
 
  friend bool operator<(const Camera& lhs, const fp& timestamp) { return lhs.timestamp_ < timestamp; }
  friend bool operator<(const fp& timestamp, const Camera& rhs) { return timestamp < rhs.timestamp_; }
 
  cv::Mat image_;      
  cv::Mat mask_;       
  fp timestamp_ = -1;  
};
 
struct TriangulatedFeatures
{
  friend bool operator<(const TriangulatedFeatures& lhs, const TriangulatedFeatures& rhs)
  {
    return lhs.timestamp_ < rhs.timestamp_;
  }
 
  friend bool operator<(const TriangulatedFeatures& lhs, const fp& timestamp) { return lhs.timestamp_ < timestamp; }
  friend bool operator<(const fp& timestamp, const TriangulatedFeatures& rhs) { return timestamp < rhs.timestamp_; }
 
  Features features_;            
  std::vector<Vector3> points_;  
  fp timestamp_ = -1;            
};
 
}  // namespace msceqf
 
#endif  // INPUT_HPP