Program Listing for File EurocDataset.h

↰ Return to documentation for file (include/mola_input_euroc_dataset/EurocDataset.h)

/*               _
 _ __ ___   ___ | | __ _
| '_ ` _ \ / _ \| |/ _` | Modular Optimization framework for
| | | | | | (_) | | (_| | Localization and mApping (MOLA)
|_| |_| |_|\___/|_|\__,_| https://github.com/MOLAorg/mola

 Copyright (C) 2018-2026 Jose Luis Blanco, University of Almeria,
                         and individual contributors.
 SPDX-License-Identifier: GPL-3.0
 See LICENSE for full license information.
*/

#pragma once

#include <mola_kernel/interfaces/Dataset_UI.h>
#include <mola_kernel/interfaces/RawDataSourceBase.h>
#include <mrpt/core/Clock.h>
#include <mrpt/img/TCamera.h>
#include <mrpt/math/TPose3D.h>

#include <array>
#include <map>
#include <string>
#include <variant>

namespace mola
{
struct SensorCamera
{
  std::string                  sensor_name;
  std::string                  img_file_name;
  uint8_t                      cam_idx;
  mrpt::obs::CObservation::Ptr obs;
};
struct SensorIMU
{
  std::string                  sensor_name;
  double                       wx, wy, wz, accx, accy, accz;
  mrpt::obs::CObservation::Ptr obs;
};
using SensorEntry       = std::variant<std::monostate, SensorCamera, SensorIMU>;
using euroc_timestamp_t = uint64_t;
using euroc_dataset_t   = std::multimap<euroc_timestamp_t, SensorEntry>;

class EurocDataset : public RawDataSourceBase, public Dataset_UI

{
  DEFINE_MRPT_OBJECT(EurocDataset, mola)

 public:
  EurocDataset();

  // See docs in base class
  void spinOnce() override;

  // Virtual interface of Dataset_UI (see docs in derived class)
  size_t datasetUI_size() const override { return dataset_.size(); }
  size_t datasetUI_lastQueriedTimestep() const override
  {
    auto lck = mrpt::lockHelper(dataset_ui_mtx_);
    return last_used_tim_index_;
  }
  double datasetUI_playback_speed() const override
  {
    auto lck = mrpt::lockHelper(dataset_ui_mtx_);
    return time_warp_scale_;
  }
  void datasetUI_playback_speed(double speed) override
  {
    auto lck         = mrpt::lockHelper(dataset_ui_mtx_);
    time_warp_scale_ = speed;
  }
  bool datasetUI_paused() const override
  {
    auto lck = mrpt::lockHelper(dataset_ui_mtx_);
    return paused_;
  }
  void datasetUI_paused(bool paused) override
  {
    auto lck = mrpt::lockHelper(dataset_ui_mtx_);
    paused_  = paused;
  }
  void datasetUI_teleport(size_t timestep) override
  {
    auto lck       = mrpt::lockHelper(dataset_ui_mtx_);
    teleport_here_ = timestep;
  }

 protected:
  // See docs in base class
  void initialize_rds(const Yaml& cfg) override;

 private:
  std::string                        base_dir_;
  std::string                        sequence_;
  std::array<mrpt::img::TCamera, 2>  cam_intrinsics_;
  std::array<mrpt::math::TPose3D, 2> cam_poses_;
  euroc_dataset_t                    dataset_;
  euroc_dataset_t::iterator          dataset_next_;
  size_t                             dataset_cur_idx_ = 0;

  std::optional<mrpt::Clock::time_point> last_play_wallclock_time_;
  double                                 last_dataset_time_ = 0;

  // double              replay_time_{.0};
  std::string seq_dir_;

  void build_dataset_entry_obs(SensorCamera& s);
  void build_dataset_entry_obs(SensorIMU& s);

  mutable timestep_t    last_used_tim_index_ = 0;
  bool                  paused_              = false;
  double                time_warp_scale_     = 1.0;
  std::optional<size_t> teleport_here_;
  mutable std::mutex    dataset_ui_mtx_;
};

}  // namespace mola