Program Listing for File MulranDataset.h
↰ Return to documentation for file (include/mola_input_mulran_dataset/MulranDataset.h
)
/* -------------------------------------------------------------------------
* A Modular Optimization framework for Localization and mApping (MOLA)
* Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
* See LICENSE for license information.
* ------------------------------------------------------------------------- */
#pragma once
#include <mola_kernel/interfaces/Dataset_UI.h>
#include <mola_kernel/interfaces/OfflineDatasetSource.h>
#include <mola_kernel/interfaces/RawDataSourceBase.h>
#include <mrpt/core/Clock.h>
#include <mrpt/img/TCamera.h>
#include <mrpt/math/CMatrixDynamic.h>
#include <mrpt/math/TPose3D.h>
#include <mrpt/obs/CObservationGPS.h>
#include <mrpt/obs/CObservationPointCloud.h>
#include <mrpt/obs/obs_frwds.h>
#include <array>
// fwrd decls:
namespace mrpt::obs
{
class CObservationPointCloud;
}
namespace mola
{
class MulranDataset : public RawDataSourceBase,
public OfflineDatasetSource,
public Dataset_UI
{
DEFINE_MRPT_OBJECT(MulranDataset, mola)
public:
MulranDataset();
~MulranDataset() override = default;
static constexpr double HDOP_REFERENCE_METERS = 4.5;
// See docs in base class
void spinOnce() override;
bool hasGroundTruthTrajectory() const override
{
return !groundTruthTrajectory_.empty();
}
trajectory_t getGroundTruthTrajectory() const override
{
return groundTruthTrajectory_;
}
mrpt::obs::CObservationPointCloud::Ptr getPointCloud(timestep_t step) const;
mrpt::obs::CObservationGPS::Ptr getGPS(timestep_t step) const;
// See docs in base class:
size_t datasetSize() const override;
mrpt::obs::CSensoryFrame::Ptr datasetGetObservations(
size_t timestep) const override;
bool hasGPS() const { return !gpsCsvData_.empty(); }
// Virtual interface of Dataset_UI (see docs in derived class)
size_t datasetUI_size() const override { return datasetSize(); }
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:
bool initialized_ = false;
std::string base_dir_;
std::string sequence_;
bool lidar_to_ground_truth_1to1_ = true;
bool publish_lidar_{true};
bool publish_gps_{true};
bool publish_ground_truth_{true};
std::optional<mrpt::Clock::time_point> last_play_wallclock_time_;
double last_dataset_time_ = 0;
enum class EntryType : uint8_t
{
Invalid = 0,
Lidar,
GNSS,
GroundTruth,
};
struct Entry
{
EntryType type = EntryType::Invalid;
timestep_t lidarIdx = 0; // idx in lstPointCloudFiles_
timestep_t gpsIdx = 0; // row indices in gpsCsvData_
timestep_t gtIdx = 0; // idx in groundTruthTrajectory_
};
std::multimap<double, Entry> datasetEntries_;
std::multimap<double, Entry>::iterator replay_next_it_;
std::vector<std::string> lstPointCloudFiles_;
trajectory_t groundTruthTrajectory_;
mutable std::map<timestep_t, mrpt::obs::CObservationPointCloud::Ptr>
read_ahead_lidar_obs_;
mrpt::math::CMatrixDouble gpsCsvData_;
// I found no extrinsics for the GPS sensor in this dataset web (?):
mrpt::poses::CPose3D gpsPoseOnVehicle_ = mrpt::poses::CPose3D::Identity();
mrpt::poses::CPose3D ousterPoseOnVehicle_;
double replay_time_ = .0;
std::string seq_dir_;
void load_lidar(timestep_t step) const;
mrpt::obs::CObservationGPS::Ptr get_gps_by_row_index(size_t row) const;
void autoUnloadOldEntries() const;
static double LidarFileNameToTimestamp(const std::string& filename);
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