Skip to content

Commit 3f51617

Browse files
jlblancocjlblancoc
committed
implement UI dataset
1 parent 6c1102c commit 3f51617

File tree

4 files changed

+198
-49
lines changed

4 files changed

+198
-49
lines changed

Diff for: ‎mola_input_kitti_dataset/include/mola_input_kitti_dataset/KittiOdometryDataset.h

+58-16
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
*/
1212
#pragma once
1313

14+
#include <mola_kernel/interfaces/Dataset_UI.h>
1415
#include <mola_kernel/interfaces/OfflineDatasetSource.h>
1516
#include <mola_kernel/interfaces/RawDataSourceBase.h>
1617
#include <mrpt/core/Clock.h>
@@ -35,13 +36,15 @@ namespace mola
3536
* - `lidar`: Velodyne 3D LIDAR
3637
* - Ground truth poses
3738
*
38-
* If the option `clouds_as_organized_points` is true (default), point cloud
39+
* If the option `clouds_as_organized_points` is true, point cloud
3940
* are published as mrpt::obs::CObservationRotatingScan.
40-
* Otherwise, they are published as mrpt::obs::CObservationPointCloud.
41+
* Otherwise (default), they are published as mrpt::obs::CObservationPointCloud
42+
* with X,Y,Z,I channels.
4143
*
4244
* \ingroup mola_input_kitti_dataset_grp */
4345
class KittiOdometryDataset : public RawDataSourceBase,
44-
public OfflineDatasetSource
46+
public OfflineDatasetSource,
47+
public Dataset_UI
4548
{
4649
DEFINE_MRPT_OBJECT(KittiOdometryDataset, mola)
4750

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

84+
// Virtual interface of Dataset_UI (see docs in derived class)
85+
size_t datasetUI_size() const override { return datasetSize(); }
86+
size_t datasetUI_lastQueriedTimestep() const override
87+
{
88+
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
89+
return last_used_tim_index_;
90+
}
91+
double datasetUI_playback_speed() const override
92+
{
93+
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
94+
return time_warp_scale_;
95+
}
96+
void datasetUI_playback_speed(double speed) override
97+
{
98+
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
99+
time_warp_scale_ = speed;
100+
}
101+
bool datasetUI_paused() const override
102+
{
103+
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
104+
return paused_;
105+
}
106+
void datasetUI_paused(bool paused) override
107+
{
108+
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
109+
paused_ = paused;
110+
}
111+
void datasetUI_teleport(size_t timestep) override
112+
{
113+
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
114+
teleport_here_ = timestep;
115+
}
116+
81117
/** See:
82118
* "IMLS-SLAM: scan-to-model matching based on 3D data", JE Deschaud, 2018.
83119
*/
84120
double VERTICAL_ANGLE_OFFSET = mrpt::DEG2RAD(0.205);
85121

86122
private:
87-
bool initialized_ = false;
88-
std::string base_dir_; //!< base dir for "sequences/*".
89-
std::string sequence_; //!< "00", "01", ...
90-
bool clouds_as_organized_points_ = false;
91-
unsigned int range_matrix_column_count_ = 2000;
92-
unsigned int range_matrix_row_count_ = 64;
93-
mrpt::Clock::time_point replay_begin_time_{};
94-
timestep_t replay_next_tim_index_{0};
95-
bool replay_started_{false};
96-
bool publish_lidar_{true};
97-
bool publish_ground_truth_{true};
98-
std::array<bool, 4> publish_image_{{true, true, true, true}};
99-
double time_warp_scale_{1.0};
123+
bool initialized_ = false;
124+
std::string base_dir_; //!< base dir for "sequences/*".
125+
std::string sequence_; //!< "00", "01", ...
126+
bool clouds_as_organized_points_ = false;
127+
unsigned int range_matrix_column_count_ = 2000;
128+
unsigned int range_matrix_row_count_ = 64;
129+
timestep_t replay_next_tim_index_{0};
130+
bool publish_lidar_{true};
131+
bool publish_ground_truth_{true};
132+
std::array<bool, 4> publish_image_{{true, true, true, true}};
100133
std::array<mrpt::img::TCamera, 4> cam_intrinsics_;
101134
std::array<mrpt::math::TPose3D, 4> cam_poses_; //!< wrt vehicle origin
102135

136+
std::optional<mrpt::Clock::time_point> last_play_wallclock_time_;
137+
double last_dataset_time_ = 0;
138+
103139
std::array<std::vector<std::string>, 4> lst_image_;
104140
std::vector<std::string> lst_velodyne_;
105141
mrpt::math::CMatrixDouble groundTruthPoses_;
@@ -113,6 +149,12 @@ class KittiOdometryDataset : public RawDataSourceBase,
113149
double replay_time_{.0};
114150
std::string seq_dir_;
115151

152+
mutable timestep_t last_used_tim_index_ = 0;
153+
bool paused_ = false;
154+
double time_warp_scale_ = 1.0;
155+
std::optional<size_t> teleport_here_;
156+
mutable std::mutex dataset_ui_mtx_;
157+
116158
void load_img(const unsigned int cam_idx, const timestep_t step) const;
117159
void load_lidar(timestep_t step) const;
118160

Diff for: ‎mola_input_kitti_dataset/src/KittiOdometryDataset.cpp

+41-10
Original file line numberDiff line numberDiff line change
@@ -101,6 +101,8 @@ void KittiOdometryDataset::initialize(const Yaml& c)
101101
// Optional params with default values:
102102
time_warp_scale_ =
103103
cfg.getOrDefault<double>("time_warp_scale", time_warp_scale_);
104+
paused_ = cfg.getOrDefault<bool>("start_paused", paused_);
105+
104106
publish_lidar_ = cfg.getOrDefault<bool>("publish_lidar", publish_lidar_);
105107

106108
publish_ground_truth_ =
@@ -299,17 +301,35 @@ void KittiOdometryDataset::spinOnce()
299301

300302
ProfilerEntry tleg(profiler_, "spinOnce");
301303

304+
const auto tNow = mrpt::Clock::now();
305+
302306
// Starting time:
303-
if (!replay_started_)
304-
{
305-
replay_begin_time_ = mrpt::Clock::now();
306-
replay_started_ = true;
307-
}
307+
if (!last_play_wallclock_time_) last_play_wallclock_time_ = tNow;
308308

309309
// get current replay time:
310-
const double t =
311-
mrpt::system::timeDifference(replay_begin_time_, mrpt::Clock::now()) *
312-
time_warp_scale_;
310+
auto lckUIVars = mrpt::lockHelper(dataset_ui_mtx_);
311+
const double time_warp_scale = time_warp_scale_;
312+
const bool paused = paused_;
313+
const auto teleport_here = teleport_here_;
314+
teleport_here_.reset();
315+
lckUIVars.unlock();
316+
317+
double dt = mrpt::system::timeDifference(*last_play_wallclock_time_, tNow) *
318+
time_warp_scale;
319+
last_play_wallclock_time_ = tNow;
320+
321+
// override by an special teleport order?
322+
if (teleport_here.has_value() && *teleport_here < lst_timestamps_.size())
323+
{
324+
replay_next_tim_index_ = *teleport_here;
325+
last_dataset_time_ = lst_timestamps_[replay_next_tim_index_];
326+
}
327+
else
328+
{
329+
if (paused) return;
330+
// move forward replayed dataset time:
331+
last_dataset_time_ += dt;
332+
}
313333

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

332352
// We have to publish all observations until "t":
333353
while (replay_next_tim_index_ < lst_timestamps_.size() &&
334-
t >= lst_timestamps_[replay_next_tim_index_])
354+
last_dataset_time_ >= lst_timestamps_[replay_next_tim_index_])
335355
{
336356
MRPT_LOG_DEBUG_STREAM(
337357
"Sending observations for replay time: "
338-
<< mrpt::system::formatTimeInterval(t));
358+
<< mrpt::system::formatTimeInterval(last_dataset_time_));
339359

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

409+
{
410+
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
411+
last_used_tim_index_ =
412+
replay_next_tim_index_ > 0 ? replay_next_tim_index_ - 1 : 0;
413+
}
414+
389415
// Read ahead to save delays in the next iteration:
390416
if (replay_next_tim_index_ < lst_timestamps_.size())
391417
{
@@ -628,6 +654,11 @@ size_t KittiOdometryDataset::datasetSize() const
628654
mrpt::obs::CSensoryFrame::Ptr KittiOdometryDataset::datasetGetObservations(
629655
size_t timestep) const
630656
{
657+
{
658+
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
659+
last_used_tim_index_ = timestep;
660+
}
661+
631662
auto sf = mrpt::obs::CSensoryFrame::Create();
632663

633664
for (size_t i = 0; i < publish_image_.size(); i++)

Diff for: ‎mola_input_mulran_dataset/include/mola_input_mulran_dataset/MulranDataset.h

+53-11
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
*/
1212
#pragma once
1313

14+
#include <mola_kernel/interfaces/Dataset_UI.h>
1415
#include <mola_kernel/interfaces/OfflineDatasetSource.h>
1516
#include <mola_kernel/interfaces/RawDataSourceBase.h>
1617
#include <mrpt/core/Clock.h>
@@ -74,7 +75,9 @@ namespace mola
7475
*
7576
* \ingroup mola_input_mulran_dataset_grp
7677
*/
77-
class MulranDataset : public RawDataSourceBase, public OfflineDatasetSource
78+
class MulranDataset : public RawDataSourceBase,
79+
public OfflineDatasetSource,
80+
public Dataset_UI
7881
{
7982
DEFINE_MRPT_OBJECT(MulranDataset, mola)
8083

@@ -112,17 +115,50 @@ class MulranDataset : public RawDataSourceBase, public OfflineDatasetSource
112115

113116
bool hasGPS() const { return !gpsCsvData_.empty(); }
114117

118+
// Virtual interface of Dataset_UI (see docs in derived class)
119+
size_t datasetUI_size() const override { return datasetSize(); }
120+
size_t datasetUI_lastQueriedTimestep() const override
121+
{
122+
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
123+
return last_used_tim_index_;
124+
}
125+
double datasetUI_playback_speed() const override
126+
{
127+
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
128+
return time_warp_scale_;
129+
}
130+
void datasetUI_playback_speed(double speed) override
131+
{
132+
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
133+
time_warp_scale_ = speed;
134+
}
135+
bool datasetUI_paused() const override
136+
{
137+
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
138+
return paused_;
139+
}
140+
void datasetUI_paused(bool paused) override
141+
{
142+
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
143+
paused_ = paused;
144+
}
145+
void datasetUI_teleport(size_t timestep) override
146+
{
147+
auto lck = mrpt::lockHelper(dataset_ui_mtx_);
148+
teleport_here_ = timestep;
149+
}
150+
115151
private:
116-
bool initialized_ = false;
117-
std::string base_dir_; //!< base dir for "sequences/*".
118-
std::string sequence_; //!< "00", "01", ...
119-
bool lidar_to_ground_truth_1to1_ = true;
120-
mrpt::Clock::time_point replay_begin_time_{};
121-
bool replay_started_{false};
122-
bool publish_lidar_{true};
123-
bool publish_gps_{true};
124-
bool publish_ground_truth_{true};
125-
double time_warp_scale_{1.0};
152+
bool initialized_ = false;
153+
std::string base_dir_; //!< base dir for "sequences/*".
154+
std::string sequence_; //!< "00", "01", ...
155+
bool lidar_to_ground_truth_1to1_ = true;
156+
bool publish_lidar_{true};
157+
bool publish_gps_{true};
158+
bool publish_ground_truth_{true};
159+
160+
std::optional<mrpt::Clock::time_point> last_play_wallclock_time_;
161+
double last_dataset_time_ = 0;
126162

127163
enum class EntryType : uint8_t
128164
{
@@ -166,6 +202,12 @@ class MulranDataset : public RawDataSourceBase, public OfflineDatasetSource
166202
void autoUnloadOldEntries() const;
167203

168204
static double LidarFileNameToTimestamp(const std::string& filename);
205+
206+
mutable timestep_t last_used_tim_index_ = 0;
207+
bool paused_ = false;
208+
double time_warp_scale_ = 1.0;
209+
std::optional<size_t> teleport_here_;
210+
mutable std::mutex dataset_ui_mtx_;
169211
};
170212

171213
} // namespace mola

0 commit comments

Comments
 (0)