Program Listing for File imu-measurements-buffer.h

Return to documentation for file (common/vio-common/include/vio-common/imu-measurements-buffer.h)

#ifndef VIO_COMMON_IMU_MEASUREMENTS_BUFFER_H_
#define VIO_COMMON_IMU_MEASUREMENTS_BUFFER_H_

#include <algorithm>
#include <atomic>
#include <condition_variable>
#include <memory>
#include <mutex>
#include <utility>

#include <Eigen/Dense>
#include <glog/logging.h>
#include <maplab-common/macros.h>
#include <maplab-common/temporal-buffer.h>

#include "vio-common/vio-types.h"

namespace vio_common {

class ImuMeasurementBuffer {
 public:
  MAPLAB_POINTER_TYPEDEFS(ImuMeasurementBuffer);
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  enum class QueryResult {
    kDataAvailable,
    kDataNotYetAvailable,
    kDataNeverAvailable,
    kQueueShutdown
  };

  explicit ImuMeasurementBuffer(int64_t buffer_length_ns)
      : buffer_(buffer_length_ns), shutdown_(false) {}
  ~ImuMeasurementBuffer() {
    shutdown();
  }

  inline void shutdown();
  inline size_t size() const;
  inline void clear();

  inline void addMeasurement(
      int64_t timestamp_nanoseconds, const vio::ImuData& imu_measurement);
  inline void addMeasurements(
      const Eigen::Matrix<int64_t, 1, Eigen::Dynamic>& timestamps_nanoseconds,
      const Eigen::Matrix<double, 6, Eigen::Dynamic>& imu_measurements);

  QueryResult getImuDataInterpolatedBorders(
      int64_t timestamp_from, int64_t timestamp_to,
      Eigen::Matrix<int64_t, 1, Eigen::Dynamic>* imu_timestamps,
      Eigen::Matrix<double, 6, Eigen::Dynamic>* imu_measurements) const;

  QueryResult getImuDataInterpolatedBordersBlocking(
      int64_t timestamp_ns_from, int64_t timestamp_ns_to,
      int64_t wait_timeout_nanoseconds,
      Eigen::Matrix<int64_t, 1, Eigen::Dynamic>* imu_timestamps,
      Eigen::Matrix<double, 6, Eigen::Dynamic>* imu_measurements) const;

  bool getNewestTime(int64_t* timestamp_ns) const;
  bool getOldestTime(int64_t* timestamp_ns) const;

 private:
  QueryResult isDataAvailableUpToImpl(
      int64_t timestamp_ns_from, int64_t timestamp_ns_to) const;

  QueryResult getImuDataInterpolatedBordersImpl(
      int64_t timestamp_ns_from, int64_t timestamp_ns_to,
      Eigen::Matrix<int64_t, 1, Eigen::Dynamic>* imu_timestamps,
      Eigen::Matrix<double, 6, Eigen::Dynamic>* imu_measurements) const;

  typedef std::pair<const int64_t, vio::ImuMeasurement> BufferElement;
  typedef Eigen::aligned_allocator<BufferElement> BufferAllocator;
  typedef common::TemporalBuffer<vio::ImuMeasurement, BufferAllocator> Buffer;

  Buffer buffer_;
  mutable std::mutex m_buffer_;
  mutable std::condition_variable cv_new_measurement_;
  std::atomic<bool> shutdown_;
};
}  // namespace vio_common
#include <vio-common/internal/imu-measurements-buffer-inl.h>
#endif  // VIO_COMMON_IMU_MEASUREMENTS_BUFFER_H_