diff --git a/mola_input_paris_luco_dataset/include/mola_input_paris_luco_dataset/ParisLucoDataset.h b/mola_input_paris_luco_dataset/include/mola_input_paris_luco_dataset/ParisLucoDataset.h index 00f76c4a..571d0c55 100644 --- a/mola_input_paris_luco_dataset/include/mola_input_paris_luco_dataset/ParisLucoDataset.h +++ b/mola_input_paris_luco_dataset/include/mola_input_paris_luco_dataset/ParisLucoDataset.h @@ -28,7 +28,12 @@ class CObservationPointCloud; namespace mola { -/** RawDataSource from Paris Luco Dataset +/** RawDataSource from Paris Luco Dataset. + * + * Dataset available from: https://github.com/jedeschaud/ct_icp + * + * The LiDAR sensor is a HDL-32, recording data at the Luxembourg Garden + * (Paris). * * Point clouds are published as mrpt::obs::CObservationPointCloud * with clouds of types mrpt::maps::CPointsMapXYZIRT, with these populated @@ -38,6 +43,8 @@ namespace mola * - `T`: Time of each point, in range [-0.05, 0.05] seconds (scan rate=10 Hz), * such that "t=0" (the observation/scan timestamp) corresponds to the moment * the scanner is facing forward. + * - `R`: ring_id (0-31). It was not provided by the original dataset, but it is + * reconstructed from point elevation data in this package. * * Expected contents under `base_dir` directory: * diff --git a/mola_input_paris_luco_dataset/src/ParisLucoDataset.cpp b/mola_input_paris_luco_dataset/src/ParisLucoDataset.cpp index ef36f6eb..25aec65e 100644 --- a/mola_input_paris_luco_dataset/src/ParisLucoDataset.cpp +++ b/mola_input_paris_luco_dataset/src/ParisLucoDataset.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -250,6 +251,45 @@ void ParisLucoDataset::load_lidar(timestep_t step) const return t - shiftTime; }); + // Fix missing RING_ID: ParisLuco does not have a RING_ID field, + // but we can generate it from the timestamps + pitch angle: + ASSERT_(pts->hasRingField()); + ASSERT_EQUAL_( + pts->getPointsBufferRef_ring()->size(), + pts->getPointsBufferRef_timestamp()->size()); + std::map> + histogram; + + // Equivalent matlab code: + // depth = sqrt(D(:,1).^2 + D(:,2).^2); % (x,y) only + // pitch = asin((D(:,3)) ./ depth); + // [nn,xx] =hist(pitch,128); + + const auto& xs = pts->getPointsBufferRef_x(); + const auto& ys = pts->getPointsBufferRef_y(); + const auto& zs = pts->getPointsBufferRef_z(); + const size_t nPts = xs.size(); + + const float fov_down = mrpt::DEG2RAD(36.374f); + const float fov = mrpt::DEG2RAD(10.860f) + fov_down; + + for (size_t i = 0; i < nPts; i++) + { + const float depth = sqrt(mrpt::square(xs[i]) + mrpt::square(ys[i])); + if (depth < 0.05) continue; + const float pitch = asin(zs[i] / depth); + + int iP = mrpt::round(31 * (pitch + fov_down) / fov); + mrpt::saturate(iP, 0, 31); + + auto& vec = histogram[iP]; + vec[(*Ts)[i]] = i; + } + + auto& Rs = *pts->getPointsBufferRef_ring(); + for (const auto& [ringId, vec] : histogram) + for (const auto& [time, idx] : vec) Rs[idx] = ringId; + // Lidar is at the origin of the vehicle frame: obs->sensorPose = mrpt::poses::CPose3D(); obs->timestamp = mrpt::Clock::fromDouble(lst_timestamps_.at(step));