mola_viz: support visualizing velodyne observations

MOLAorg · Dec 29, 2023 · 7d226ca · 7d226ca
1 parent b347618
commit 7d226ca
Show file tree

Hide file tree

Showing 3 changed files with 57 additions and 16 deletions.
diff --git a/mola_kernel/src/interfaces/RawDataSourceBase.cpp b/mola_kernel/src/interfaces/RawDataSourceBase.cpp
@@ -225,19 +225,28 @@ void RawDataSourceBase::prepareObservationBeforeFrontEnds(
     using namespace mrpt::obs;
 
     // These operations are optional:
-    if (!force_load_lazy_load_) return;
-
-    // for delay-load data:
-    obs->load();
+    if (!force_load_lazy_load_) { obs->load(); }
 
     // Sensor-specific:
     if (auto o_velo = mrpt::ptr_cast<CObservationVelodyneScan>::from(obs);
         o_velo)
     {
-        if (!o_velo->point_cloud.size()) o_velo->generatePointCloud();
+        if (!o_velo->point_cloud.size())
+        {
+            // Generate point timestamps & RING ID:
+            mrpt::obs::CObservationVelodyneScan::TGeneratePointCloudParameters
+                p;
+            p.generatePerPointTimestamp = true;
+            p.generatePointsForLaserID  = true;
+            o_velo->generatePointCloud(p);
+        }
+
         const auto& pc = o_velo->point_cloud;
         ASSERT_EQUAL_(pc.x.size(), pc.y.size());
         ASSERT_EQUAL_(pc.x.size(), pc.z.size());
+        ASSERT_EQUAL_(pc.x.size(), pc.intensity.size());
+        ASSERT_EQUAL_(pc.x.size(), pc.laser_id.size());
+        ASSERT_EQUAL_(pc.x.size(), pc.timestamp.size());
     }
     MRPT_TRY_END
 }
diff --git a/mola_viz/src/MolaViz.cpp b/mola_viz/src/MolaViz.cpp
@@ -33,12 +33,14 @@
 #include <mrpt/containers/yaml.h>
 #include <mrpt/core/initializer.h>
 #include <mrpt/core/lock_helper.h>
+#include <mrpt/maps/CPointsMapXYZI.h>
 #include <mrpt/maps/CSimplePointsMap.h>
 #include <mrpt/obs/CObservation2DRangeScan.h>
 #include <mrpt/obs/CObservation3DRangeScan.h>
 #include <mrpt/obs/CObservationImage.h>
 #include <mrpt/obs/CObservationPointCloud.h>
 #include <mrpt/obs/CObservationRotatingScan.h>
+#include <mrpt/obs/CObservationVelodyneScan.h>
 #include <mrpt/opengl/CGridPlaneXY.h>
 #include <mrpt/opengl/COpenGLScene.h>
 #include <mrpt/opengl/CPointCloudColoured.h>
@@ -208,6 +210,7 @@ void gui_handler_images(
 // CObservation2DRangeScan
 // CObservation3DRangeScan
 // CObservationRotatingScan
+// CObservationVelodyneScan
 void gui_handler_point_cloud(
     const mrpt::rtti::CObject::Ptr& o, nanogui::Window* w,
     MolaViz::window_name_t parentWin, MolaViz* instance)
@@ -249,7 +252,7 @@ void gui_handler_point_cloud(
     if (auto objPc = std::dynamic_pointer_cast<CObservationPointCloud>(o);
         objPc)
     {
-        // objPc->load();
+        objPc->load();
         if (!objPc->pointcloud) return;
         glPc->loadFromPointsMap(objPc->pointcloud.get());
 
@@ -308,6 +311,29 @@ void gui_handler_point_cloud(
 
         gui_handler_show_common_sensor_info(*obj2D, w);
     }
+    else if (auto objVel =
+                 std::dynamic_pointer_cast<CObservationVelodyneScan>(o);
+             objVel)
+    {
+        if (objVel->point_cloud.size() == 0) return;
+
+        mrpt::maps::CPointsMapXYZI pts;
+        const auto&                pc = objVel->point_cloud;
+        const size_t               N  = pc.size();
+        pts.resize(N);
+        for (size_t i = 0; i < N; i++)
+        {
+            pts.setPoint(i, pc.x[i], pc.y[i], pc.z[i]);
+            pts.setPointIntensity(i, pc.intensity[i] / 255.0f);
+        }
+        glPc->loadFromPointsMap(&pts);
+
+        gui_handler_show_common_sensor_info(
+            *objVel, w,
+            {
+                mrpt::format("Point count: %zu", N),
+            });
+    }
     else
         return;
 
@@ -323,16 +349,14 @@ MRPT_INITIALIZER(do_register_MolaViz)
     MOLA_REGISTER_MODULE(MolaViz);
 
     // Register GUI handlers for common sensor types:
-    MolaViz::register_gui_handler(
-        "mrpt::obs::CObservationImage", &gui_handler_images);
-    MolaViz::register_gui_handler(
-        "mrpt::obs::CObservationPointCloud", &gui_handler_point_cloud);
-    MolaViz::register_gui_handler(
-        "mrpt::obs::CObservation3DRangeScan", &gui_handler_point_cloud);
-    MolaViz::register_gui_handler(
-        "mrpt::obs::CObservation2DRangeScan", &gui_handler_point_cloud);
-    MolaViz::register_gui_handler(
-        "mrpt::obs::CObservationRotatingScan", &gui_handler_point_cloud);
+    // clang-format off
+    MolaViz::register_gui_handler("mrpt::obs::CObservationImage", &gui_handler_images);
+    MolaViz::register_gui_handler("mrpt::obs::CObservationPointCloud",   &gui_handler_point_cloud);
+    MolaViz::register_gui_handler("mrpt::obs::CObservation3DRangeScan",  &gui_handler_point_cloud);
+    MolaViz::register_gui_handler("mrpt::obs::CObservation2DRangeScan",  &gui_handler_point_cloud);
+    MolaViz::register_gui_handler("mrpt::obs::CObservationRotatingScan", &gui_handler_point_cloud);
+    MolaViz::register_gui_handler("mrpt::obs::CObservationVelodyneScan", &gui_handler_point_cloud);
+    // clang-format on
 }
 
 MolaViz*                     MolaViz::instance_ = nullptr;

diff --git a/mola_yaml/include/mola_yaml/yaml_helpers.h b/mola_yaml/include/mola_yaml/yaml_helpers.h
@@ -74,6 +74,10 @@ struct YAMLParseOptions
 #define YAML_LOAD_OPT2(_varname, _type) \
     _varname = cfg.getOrDefault<_type>(#_varname, _varname)
 
+#define YAML_LOAD_OPT_DEG2(_varname, _type) \
+    _varname = mrpt::DEG2RAD(               \
+        cfg.getOrDefault<_type>(#_varname, mrpt::RAD2DEG(_varname)))
+
 /** Use `YAML_LOAD_MEMBER_OPT(foo,double);` to load YAML var `foo` into `foo_`
  */
 #define YAML_LOAD_MEMBER_OPT(_varname, _type) \
@@ -89,6 +93,10 @@ struct YAMLParseOptions
     ENSURE_YAML_ENTRY_EXISTS(cfg, #_varname); \
     YAML_LOAD_OPT2(_varname, _type)
 
+#define YAML_LOAD_REQ_DEG2(_varname, _type)   \
+    ENSURE_YAML_ENTRY_EXISTS(cfg, #_varname); \
+    YAML_LOAD_OPT_DEG2(_varname, _type)
+
 /** Use `YAML_LOAD_MEMBER_REQ(foo,double);` to load YAML var `foo` into `foo_`
  */
 #define YAML_LOAD_MEMBER_REQ(_varname, _type) \