ParisLuco dataset: reconstruct ringId field

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:
  *

mola_input_paris_luco_dataset/src/ParisLucoDataset.cpp

@@ -19,6 +19,7 @@
 #include <mola_yaml/yaml_helpers.h>
 #include <mrpt/containers/yaml.h>
 #include <mrpt/core/initializer.h>
+#include <mrpt/core/round.h>
 #include <mrpt/maps/CPointsMapXYZIRT.h>
 #include <mrpt/obs/CObservationImage.h>
 #include <mrpt/obs/CObservationPointCloud.h>
@@ -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<int /*ring*/, std::map<float /*time*/, size_t /*index*/>>
+        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));