From bfd75f9093c03d90f8ac4c53748b67aeb007783a Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Tue, 29 Aug 2023 02:39:10 +0200 Subject: [PATCH] Add LIDAR 3D model: Helios 32. Removed LIDAR3D fbo_nrows parameter, automatically computed now from geometry solutions. --- models/README.md | 2 ++ .../simulator/include/mvsim/Sensors/Lidar3D.h | 3 +- modules/simulator/src/Sensors/Lidar3D.cpp | 33 +++++++++++-------- .../definitions/helios-32.sensor.xml | 31 +++++++++++++++++ .../definitions/ouster-os1.sensor.xml | 6 ++-- .../definitions/velodyne-vlp16.sensor.xml | 6 ++-- mvsim_tutorial/demo_elevation_map.world.xml | 2 +- mvsim_tutorial/demo_greenhouse.world.xml | 6 +++- mvsim_tutorial/demo_warehouse.world.xml | 10 ++++-- 9 files changed, 76 insertions(+), 23 deletions(-) create mode 100644 mvsim_tutorial/definitions/helios-32.sensor.xml diff --git a/models/README.md b/models/README.md index 96037f1b..91b5a8a0 100644 --- a/models/README.md +++ b/models/README.md @@ -1,3 +1,5 @@ +These 3D models are used in vehicle and sensor definitions in [mvsim_tutorials/definitions](../mvsim_tutorial/definitions/). + Attributions: - `jackal_description`: Files from [jackal](https://github.com/jackal/jackal) repository. diff --git a/modules/simulator/include/mvsim/Sensors/Lidar3D.h b/modules/simulator/include/mvsim/Sensors/Lidar3D.h index 1d96a06e..adbf142d 100644 --- a/modules/simulator/include/mvsim/Sensors/Lidar3D.h +++ b/modules/simulator/include/mvsim/Sensors/Lidar3D.h @@ -69,7 +69,8 @@ class Lidar3D : public SensorBase float maxRange_ = 80.0f; double vertical_fov_ = mrpt::DEG2RAD(30.0); int vertNumRays_ = 16, horzNumRays_ = 180; - int fbo_nrows_ = vertNumRays_ * 20; + double horzResolutionFactor_ = 1.0; + double vertResolutionFactor_ = 1.0; /** Last simulated scan */ mrpt::obs::CObservationPointCloud::Ptr last_scan2gui_, last_scan_; diff --git a/modules/simulator/src/Sensors/Lidar3D.cpp b/modules/simulator/src/Sensors/Lidar3D.cpp index 9753c15e..62ca2161 100644 --- a/modules/simulator/src/Sensors/Lidar3D.cpp +++ b/modules/simulator/src/Sensors/Lidar3D.cpp @@ -18,6 +18,8 @@ #include #include +//#include "rapidxml_print.hpp" + #if MRPT_VERSION >= 0x270 #include #endif @@ -57,8 +59,10 @@ void Lidar3D::loadConfigFrom(const rapidxml::xml_node* root) params["vert_fov_degrees"] = TParamEntry("%lf_deg", &vertical_fov_); params["vert_nrays"] = TParamEntry("%i", &vertNumRays_); params["horz_nrays"] = TParamEntry("%i", &horzNumRays_); - - params["fbo_nrows"] = TParamEntry("%i", &fbo_nrows_); + params["horz_resolution_factor"] = + TParamEntry("%lf", &horzResolutionFactor_); + params["vert_resolution_factor"] = + TParamEntry("%lf", &vertResolutionFactor_); // Parse XML params: parse_xmlnode_children_as_param(*root, params, varValues_); @@ -123,8 +127,6 @@ void Lidar3D::internalGuiUpdate( if (last_scan2gui_ && last_scan2gui_->pointcloud) { glPoints_->loadFromPointsMap(last_scan2gui_->pointcloud.get()); - gl_sensor_origin_corner_->setPose(last_scan2gui_->sensorPose); - last_scan2gui_.reset(); } } @@ -218,21 +220,26 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) // Create FBO on first use, now that we are here at the GUI / OpenGL thread. constexpr double camModel_hFOV = 120.01_deg; - const int FBO_NROWS = fbo_nrows_; - // This FBO is for camModel_hFOV only: - const int FBO_NCOLS = horzNumRays_; - // worst vFOV case: at each sub-scan render corner: - const double camModel_vFOV = - std::min(179.0_deg, 2.05 * atan2(1.0, 1.0 / cos(camModel_hFOV * 0.5))); + // This FBO is for camModel_hFOV only: + // Minimum horz resolution=360deg /120 deg + const int FBO_NCOLS = + mrpt::round(horzResolutionFactor_ * horzNumRays_ / 3.0); mrpt::img::TCamera camModel; camModel.ncols = FBO_NCOLS; - camModel.nrows = FBO_NROWS; camModel.cx(camModel.ncols / 2.0); - camModel.cy(camModel.nrows / 2.0); camModel.fx(camModel.cx() / tan(camModel_hFOV * 0.5)); // tan(FOV/2)=cx/fx - camModel.fy(camModel.cy() / tan(camModel_vFOV * 0.5)); + + // worst vFOV case: at each sub-scan render corner: + // (derivation in hand notes... to be passed to a paper) + using mrpt::square; + const double tanFOVhalf = ::tan(vertical_fov_ * 0.5); + const int FBO_NROWS = vertResolutionFactor_ * 2 * tanFOVhalf * + sqrt(square(camModel.fx()) + square(camModel.cx())); + camModel.nrows = FBO_NROWS; + camModel.cy(camModel.nrows / 2.0); + camModel.fy(camModel.fx()); if (!fbo_renderer_depth_) { diff --git a/mvsim_tutorial/definitions/helios-32.sensor.xml b/mvsim_tutorial/definitions/helios-32.sensor.xml new file mode 100644 index 00000000..78548d3f --- /dev/null +++ b/mvsim_tutorial/definitions/helios-32.sensor.xml @@ -0,0 +1,31 @@ + + ${sensor_x|0.5} ${sensor_y|0.0} ${sensor_z|0.7} ${sensor_yaw|0.0} ${sensor_pitch|0.0} ${sensor_roll|0.0} + + ${vert_fov_degrees|20} + ${vert_nrays|32} + + + $f{1.0/${sensor_rate|10}} + $f{(1.0/${sensor_rate|10})/55.296e-6} + + + ${horz_resolution_factor|2.0} + ${vert_resolution_factor|2.0} + + ${sensor_std_noise|0.005} + ${min_range|0.20} + ${max_range|110.0} + + ../models/velodyne-vlp16.dae 90 + + + + /${PARENT_NAME}/${NAME} + + diff --git a/mvsim_tutorial/definitions/ouster-os1.sensor.xml b/mvsim_tutorial/definitions/ouster-os1.sensor.xml index 503655e8..cc66d2c5 100644 --- a/mvsim_tutorial/definitions/ouster-os1.sensor.xml +++ b/mvsim_tutorial/definitions/ouster-os1.sensor.xml @@ -20,9 +20,11 @@ ${sensor_period_sec|0.10} ${sensor_horz_nrays|1024} - ${fbo_nrows|2000} + + ${horz_resolution_factor|2.0} + ${vert_resolution_factor|2.0} - ${sensor_std_noise|0.025} + ${sensor_std_noise|0.005} ${min_range|0.5} ${max_range|90.0} diff --git a/mvsim_tutorial/definitions/velodyne-vlp16.sensor.xml b/mvsim_tutorial/definitions/velodyne-vlp16.sensor.xml index 8c007da2..4f8dba82 100644 --- a/mvsim_tutorial/definitions/velodyne-vlp16.sensor.xml +++ b/mvsim_tutorial/definitions/velodyne-vlp16.sensor.xml @@ -16,9 +16,11 @@ $f{60.0/${sensor_rpm|600}} $f{(60.0/${sensor_rpm|600})/55.296e-6} - ${fbo_nrows|350} + + ${horz_resolution_factor|2.0} + ${vert_resolution_factor|2.0} - ${sensor_std_noise|0.01} + ${sensor_std_noise|0.005} ${max_range|80.0} ../models/velodyne-vlp16.dae 90 diff --git a/mvsim_tutorial/demo_elevation_map.world.xml b/mvsim_tutorial/demo_elevation_map.world.xml index 09c90dfd..78e2f76e 100644 --- a/mvsim_tutorial/demo_elevation_map.world.xml +++ b/mvsim_tutorial/demo_elevation_map.world.xml @@ -68,7 +68,7 @@ --> + + - + +