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 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 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