Skip to content

Commit

Permalink
implement UI dataset
Browse files Browse the repository at this point in the history
  • Loading branch information
@jlblancoc
jlblancoc committed Feb 20, 2024
1 parent 6c1102c commit 3f51617
Show file tree
Hide file tree
Showing 4 changed files with 198 additions and 49 deletions.
74 changes: 58 additions & 16 deletions mola_input_kitti_dataset/include/mola_input_kitti_dataset/KittiOdometryDataset.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
*/
#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>
Expand All @@ -35,13 +36,15 @@ namespace mola
* - `lidar`: Velodyne 3D LIDAR
* - Ground truth poses
*
* If the option `clouds_as_organized_points` is true (default), point cloud
* If the option `clouds_as_organized_points` is true, point cloud
* are published as mrpt::obs::CObservationRotatingScan.
* Otherwise, they are published as mrpt::obs::CObservationPointCloud.
* Otherwise (default), they are published as mrpt::obs::CObservationPointCloud
* with X,Y,Z,I channels.
*
* \ingroup mola_input_kitti_dataset_grp */
class KittiOdometryDataset : public RawDataSourceBase,
public OfflineDatasetSource
public OfflineDatasetSource,
public Dataset_UI
{
DEFINE_MRPT_OBJECT(KittiOdometryDataset, mola)

Expand Down Expand Up @@ -78,28 +81,61 @@ class KittiOdometryDataset : public RawDataSourceBase,
mrpt::obs::CSensoryFrame::Ptr datasetGetObservations(
size_t timestep) const override;

// 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;
}

/** See:
* "IMLS-SLAM: scan-to-model matching based on 3D data", JE Deschaud, 2018.
*/
double VERTICAL_ANGLE_OFFSET = mrpt::DEG2RAD(0.205);

private:
bool initialized_ = false;
std::string base_dir_; //!< base dir for "sequences/*".
std::string sequence_; //!< "00", "01", ...
bool clouds_as_organized_points_ = false;
unsigned int range_matrix_column_count_ = 2000;
unsigned int range_matrix_row_count_ = 64;
mrpt::Clock::time_point replay_begin_time_{};
timestep_t replay_next_tim_index_{0};
bool replay_started_{false};
bool publish_lidar_{true};
bool publish_ground_truth_{true};
std::array<bool, 4> publish_image_{{true, true, true, true}};
double time_warp_scale_{1.0};
bool initialized_ = false;
std::string base_dir_; //!< base dir for "sequences/*".
std::string sequence_; //!< "00", "01", ...
bool clouds_as_organized_points_ = false;
unsigned int range_matrix_column_count_ = 2000;
unsigned int range_matrix_row_count_ = 64;
timestep_t replay_next_tim_index_{0};
bool publish_lidar_{true};
bool publish_ground_truth_{true};
std::array<bool, 4> publish_image_{{true, true, true, true}};
std::array<mrpt::img::TCamera, 4> cam_intrinsics_;
std::array<mrpt::math::TPose3D, 4> cam_poses_; //!< wrt vehicle origin

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

std::array<std::vector<std::string>, 4> lst_image_;
std::vector<std::string> lst_velodyne_;
mrpt::math::CMatrixDouble groundTruthPoses_;
Expand All @@ -113,6 +149,12 @@ class KittiOdometryDataset : public RawDataSourceBase,
double replay_time_{.0};
std::string seq_dir_;

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

void load_img(const unsigned int cam_idx, const timestep_t step) const;
void load_lidar(timestep_t step) const;

Expand Down
51 changes: 41 additions & 10 deletions mola_input_kitti_dataset/src/KittiOdometryDataset.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,6 +101,8 @@ void KittiOdometryDataset::initialize(const Yaml& c)
// Optional params with default values:
time_warp_scale_ =
cfg.getOrDefault<double>("time_warp_scale", time_warp_scale_);
paused_ = cfg.getOrDefault<bool>("start_paused", paused_);

publish_lidar_ = cfg.getOrDefault<bool>("publish_lidar", publish_lidar_);

publish_ground_truth_ =
Expand Down Expand Up @@ -299,17 +301,35 @@ void KittiOdometryDataset::spinOnce()

ProfilerEntry tleg(profiler_, "spinOnce");

const auto tNow = mrpt::Clock::now();

// Starting time:
if (!replay_started_)
{
replay_begin_time_ = mrpt::Clock::now();
replay_started_ = true;
}
if (!last_play_wallclock_time_) last_play_wallclock_time_ = tNow;

// get current replay time:
const double t =
mrpt::system::timeDifference(replay_begin_time_, mrpt::Clock::now()) *
time_warp_scale_;
auto lckUIVars = mrpt::lockHelper(dataset_ui_mtx_);
const double time_warp_scale = time_warp_scale_;
const bool paused = paused_;
const auto teleport_here = teleport_here_;
teleport_here_.reset();
lckUIVars.unlock();

double dt = mrpt::system::timeDifference(*last_play_wallclock_time_, tNow) *
time_warp_scale;
last_play_wallclock_time_ = tNow;

// override by an special teleport order?
if (teleport_here.has_value() && *teleport_here < lst_timestamps_.size())
{
replay_next_tim_index_ = *teleport_here;
last_dataset_time_ = lst_timestamps_[replay_next_tim_index_];
}
else
{
if (paused) return;
// move forward replayed dataset time:
last_dataset_time_ += dt;
}

if (replay_next_tim_index_ >= lst_timestamps_.size())
{
Expand All @@ -331,11 +351,11 @@ void KittiOdometryDataset::spinOnce()

// We have to publish all observations until "t":
while (replay_next_tim_index_ < lst_timestamps_.size() &&
t >= lst_timestamps_[replay_next_tim_index_])
last_dataset_time_ >= lst_timestamps_[replay_next_tim_index_])
{
MRPT_LOG_DEBUG_STREAM(
"Sending observations for replay time: "
<< mrpt::system::formatTimeInterval(t));
<< mrpt::system::formatTimeInterval(last_dataset_time_));

// Save one single timestamp for all observations, since they are in
// theory shynchronized in the Kitti datasets:
Expand Down Expand Up @@ -386,6 +406,12 @@ void KittiOdometryDataset::spinOnce()
replay_next_tim_index_++;
}

{
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
last_used_tim_index_ =
replay_next_tim_index_ > 0 ? replay_next_tim_index_ - 1 : 0;
}

// Read ahead to save delays in the next iteration:
if (replay_next_tim_index_ < lst_timestamps_.size())
{
Expand Down Expand Up @@ -628,6 +654,11 @@ size_t KittiOdometryDataset::datasetSize() const
mrpt::obs::CSensoryFrame::Ptr KittiOdometryDataset::datasetGetObservations(
size_t timestep) const
{
{
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
last_used_tim_index_ = timestep;
}

auto sf = mrpt::obs::CSensoryFrame::Create();

for (size_t i = 0; i < publish_image_.size(); i++)
Expand Down
64 changes: 53 additions & 11 deletions mola_input_mulran_dataset/include/mola_input_mulran_dataset/MulranDataset.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
*/
#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>
Expand Down Expand Up @@ -74,7 +75,9 @@ namespace mola
*
* \ingroup mola_input_mulran_dataset_grp
*/
class MulranDataset : public RawDataSourceBase, public OfflineDatasetSource
class MulranDataset : public RawDataSourceBase,
public OfflineDatasetSource,
public Dataset_UI
{
DEFINE_MRPT_OBJECT(MulranDataset, mola)

Expand Down Expand Up @@ -112,17 +115,50 @@ class MulranDataset : public RawDataSourceBase, public OfflineDatasetSource

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

private:
bool initialized_ = false;
std::string base_dir_; //!< base dir for "sequences/*".
std::string sequence_; //!< "00", "01", ...
bool lidar_to_ground_truth_1to1_ = true;
mrpt::Clock::time_point replay_begin_time_{};
bool replay_started_{false};
bool publish_lidar_{true};
bool publish_gps_{true};
bool publish_ground_truth_{true};
double time_warp_scale_{1.0};
bool initialized_ = false;
std::string base_dir_; //!< base dir for "sequences/*".
std::string sequence_; //!< "00", "01", ...
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
{
Expand Down Expand Up @@ -166,6 +202,12 @@ class MulranDataset : public RawDataSourceBase, public OfflineDatasetSource
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
Loading

0 comments on commit 3f51617

Please sign in to comment.