Add LIDAR 3D model: Helios 32. Removed LIDAR3D fbo_nrows parameter, a…

…utomatically computed now from geometry solutions.

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.

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_;

modules/simulator/src/Sensors/Lidar3D.cpp

@@ -18,6 +18,8 @@
 #include <mvsim/World.h>
 #include <mvsim/WorldElements/OccupancyGridMap.h>
 
+//#include "rapidxml_print.hpp"
+
 #if MRPT_VERSION >= 0x270
 #include <mrpt/opengl/OpenGLDepth2LinearLUTs.h>
 #endif
@@ -57,8 +59,10 @@ void Lidar3D::loadConfigFrom(const rapidxml::xml_node<char>* 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_)
 	{

mvsim_tutorial/definitions/helios-32.sensor.xml

@@ -0,0 +1,31 @@
+<sensor class="lidar3d" name="${sensor_name|lidar1}">
+    <pose_3d> ${sensor_x|0.5}  ${sensor_y|0.0}  ${sensor_z|0.7}  ${sensor_yaw|0.0} ${sensor_pitch|0.0} ${sensor_roll|0.0}</pose_3d>
+
+    <vert_fov_degrees>${vert_fov_degrees|20}</vert_fov_degrees>
+    <vert_nrays>${vert_nrays|32}</vert_nrays>
+
+    <!-- Horizontal / azimuth angular resolution:
+        The rotation of the Helios sensor configurable: 5 / 10 / 20 Hz
+        Firing timing of the sensor is fixed at 55.296 μs (=18.084 kHz).
+
+        Set the "sensor_rate" (Hz) variable from the parent XML to automatically
+        adjust the number of points per horizontal line.
+    -->
+    <sensor_period>$f{1.0/${sensor_rate|10}}</sensor_period>
+    <horz_nrays>$f{(1.0/${sensor_rate|10})/55.296e-6}</horz_nrays>
+
+    <!-- 1.0=minimum (faster), larger values=potentially finer details captured -->
+    <horz_resolution_factor>${horz_resolution_factor|2.0}</horz_resolution_factor>
+    <vert_resolution_factor>${vert_resolution_factor|2.0}</vert_resolution_factor>
+
+    <range_std_noise>${sensor_std_noise|0.005}</range_std_noise>
+    <min_range>${min_range|0.20}</min_range>
+    <max_range>${max_range|110.0}</max_range>
+
+    <visual> <model_uri>../models/velodyne-vlp16.dae</model_uri> <model_roll>90</model_roll> </visual>
+
+    <!-- Publish sensor on MVSIM ZMQ topic? (Note, this is **not** related to ROS at all) -->
+    <publish enabled="${sensor_publish|false}">
+        <publish_topic>/${PARENT_NAME}/${NAME}</publish_topic>
+    </publish>
+</sensor>

mvsim_tutorial/definitions/ouster-os1.sensor.xml

@@ -20,9 +20,11 @@
     <sensor_period>${sensor_period_sec|0.10}</sensor_period>
     <horz_nrays>${sensor_horz_nrays|1024}</horz_nrays>
 
-    <fbo_nrows>${fbo_nrows|2000}</fbo_nrows>
+    <!-- 1.0=minimum (faster), larger values=potentially finer details captured -->
+    <horz_resolution_factor>${horz_resolution_factor|2.0}</horz_resolution_factor>
+    <vert_resolution_factor>${vert_resolution_factor|2.0}</vert_resolution_factor>
 
-    <range_std_noise>${sensor_std_noise|0.025}</range_std_noise>
+    <range_std_noise>${sensor_std_noise|0.005}</range_std_noise>
     <min_range>${min_range|0.5}</min_range>
     <max_range>${max_range|90.0}</max_range>
 

mvsim_tutorial/definitions/velodyne-vlp16.sensor.xml

@@ -16,9 +16,11 @@
     <sensor_period>$f{60.0/${sensor_rpm|600}}</sensor_period>
     <horz_nrays>$f{(60.0/${sensor_rpm|600})/55.296e-6}</horz_nrays>
 
-    <fbo_nrows>${fbo_nrows|350}</fbo_nrows>
+    <!-- 1.0=minimum (faster), larger values=potentially finer details captured -->
+    <horz_resolution_factor>${horz_resolution_factor|2.0}</horz_resolution_factor>
+    <vert_resolution_factor>${vert_resolution_factor|2.0}</vert_resolution_factor>
 
-    <range_std_noise>${sensor_std_noise|0.01}</range_std_noise>
+    <range_std_noise>${sensor_std_noise|0.005}</range_std_noise>
     <max_range>${max_range|80.0}</max_range>
 
     <visual> <model_uri>../models/velodyne-vlp16.dae</model_uri> <model_roll>90</model_roll> </visual>

mvsim_tutorial/demo_elevation_map.world.xml

@@ -68,7 +68,7 @@
 
 		<include file="definitions/velodyne-vlp16.sensor.xml"
 		  sensor_x="0.15" sensor_y="0" sensor_z="0.38" sensor_yaw="0"
-		  sensor_std_noise="0.02"
+		  sensor_std_noise="0.005"
 		  sensor_publish="false"
 		  sensor_name="lidar1"
 		  sensor_rpm="600"

mvsim_tutorial/demo_greenhouse.world.xml

@@ -128,12 +128,16 @@
 		  sensor_name="scanner1"
 		/>-->
 
+		<!-- velodyne-vlp16.sensor.xml -->
+		<!-- helios-32.sensor.xml -->
 		<include file="definitions/velodyne-vlp16.sensor.xml"
 		  sensor_x="-0.34" sensor_y="0.01" sensor_z="1.53" sensor_yaw="180"
-		  sensor_std_noise="0.02"
+		  sensor_std_noise="0.005"
 		  sensor_publish="false"
 		  sensor_name="lidar1"
 		  sensor_rpm="600"
+		  horz_resolution_factor="1.0"
+		  vert_resolution_factor="1.0"
 		/>
 
 		<include file="definitions/camera.sensor.xml" 

mvsim_tutorial/demo_warehouse.world.xml

@@ -113,12 +113,16 @@
 		  sensor_name="scanner1"
 		/>
 
-		<include file="definitions/velodyne-vlp16.sensor.xml"
-		  sensor_x="0.15" sensor_z="$f{0.27 + 0.09}"
-		  sensor_std_noise="0.02"
+		<!-- velodyne-vlp16.sensor.xml -->
+		<!-- helios-32.sensor.xml -->
+		<include file="definitions/helios-32.sensor.xml"
+		  sensor_x="0.15" sensor_z="$f{0.27 + 0.08}"
+		  sensor_std_noise="0.005"
 		  sensor_publish="false"
 		  sensor_name="lidar1"
 		  sensor_rpm="600"
+		  horz_resolution_factor="2.0"
+		  vert_resolution_factor="2.0"
 		/>
 
 		<include file="definitions/camera.sensor.xml"