From 7d226ca8f3feea467f15cd654e8453868804d3bd Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 29 Dec 2023 17:31:07 +0100 Subject: [PATCH] mola_viz: support visualizing velodyne observations --- .../src/interfaces/RawDataSourceBase.cpp | 19 ++++++-- mola_viz/src/MolaViz.cpp | 46 ++++++++++++++----- mola_yaml/include/mola_yaml/yaml_helpers.h | 8 ++++ 3 files changed, 57 insertions(+), 16 deletions(-) diff --git a/mola_kernel/src/interfaces/RawDataSourceBase.cpp b/mola_kernel/src/interfaces/RawDataSourceBase.cpp index 21bae32b..9bbede43 100644 --- 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::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 index 41bacedd..a17ce751 100644 --- a/mola_viz/src/MolaViz.cpp +++ b/mola_viz/src/MolaViz.cpp @@ -33,12 +33,14 @@ #include #include #include +#include #include #include #include #include #include #include +#include #include #include #include @@ -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(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(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 index 8fee10a8..194091f1 100644 --- 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) \