Uneven 3D lidar simulation

modules/simulator/include/mvsim/Sensors/Lidar3D.h

@@ -67,7 +67,17 @@ class Lidar3D : public SensorBase
 	float viz_pointSize_ = 3.0f;
 	float minRange_ = 0.01f;
 	float maxRange_ = 80.0f;
+
+	/** vertical FOV will be symmetric above and below the horizontal line,
+	 *  unless vertical_ray_angles_str_ is set */
 	double vertical_fov_ = mrpt::DEG2RAD(30.0);
+
+	/** If not empty, will define the list of ray vertical angles, in degrees,
+	 * separated by whitespaces. Positive angles are above, negative below
+	 * the horizontal plane.
+	 */
+	std::string vertical_ray_angles_str_;
+
 	int vertNumRays_ = 16, horzNumRays_ = 180;
 	double horzResolutionFactor_ = 1.0;
 	double vertResolutionFactor_ = 1.0;
@@ -101,5 +111,9 @@ class Lidar3D : public SensorBase
 	};
 
 	std::vector<PerHorzAngleLUT> lut_;
+
+	/// Upon initialization, vertical_rays_str_ is parsed in this vector of
+	/// angles in radians.
+	std::vector<double> vertical_ray_angles_;
 };
 }  // namespace mvsim

modules/simulator/src/Sensors/Lidar3D.cpp

@@ -57,6 +57,9 @@ void Lidar3D::loadConfigFrom(const rapidxml::xml_node<char>* root)
 	params["ignore_parent_body"] = TParamEntry("%bool", &ignore_parent_body_);
 
 	params["vert_fov_degrees"] = TParamEntry("%lf_deg", &vertical_fov_);
+	params["vertical_ray_angles"] =
+		TParamEntry("%s", &vertical_ray_angles_str_);
+
 	params["vert_nrays"] = TParamEntry("%i", &vertNumRays_);
 	params["horz_nrays"] = TParamEntry("%i", &horzNumRays_);
 	params["horz_resolution_factor"] =
@@ -223,24 +226,70 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene)
 
 	// This FBO is for camModel_hFOV only:
 	// Minimum horz resolution=360deg /120 deg
-	const int FBO_NCOLS =
-		mrpt::round(horzResolutionFactor_ * horzNumRays_ / 3.0);
+	const int FBO_NCOLS = mrpt::round(
+		horzResolutionFactor_ * horzNumRays_ / (2 * M_PI / camModel_hFOV));
 
 	mrpt::img::TCamera camModel;
 	camModel.ncols = FBO_NCOLS;
 	camModel.cx(camModel.ncols / 2.0);
 	camModel.fx(camModel.cx() / tan(camModel_hFOV * 0.5));	// tan(FOV/2)=cx/fx
 
+	// Build list of vertical angles, in increasing order (first negative, below
+	// horizontal plane, final ones positive, above it):
+	if (vertical_ray_angles_.empty())
+	{
+		if (vertical_ray_angles_str_.empty())
+		{
+			// even distribution:
+			vertical_ray_angles_.resize(vertNumRays_);
+			for (int i = 0; i < vertNumRays_; i++)
+			{
+				vertical_ray_angles_[i] =
+					vertical_fov_ * (-0.5 + i * 1.0 / (vertNumRays_ - 1));
+			}
+		}
+		else
+		{
+			// custom distribution:
+			std::vector<std::string> vertAnglesStrs;
+			mrpt::system::tokenize(
+				vertical_ray_angles_str_, " \t\r\n", vertAnglesStrs);
+			ASSERT_EQUAL_(
+				vertAnglesStrs.size(), static_cast<size_t>(vertNumRays_));
+			std::set<double> angs;
+			for (const auto& s : vertAnglesStrs) angs.insert(std::stod(s));
+			ASSERT_EQUAL_(angs.size(), static_cast<size_t>(vertNumRays_));
+			for (const auto a : angs) vertical_ray_angles_.push_back(a);
+		}
+
+#if 0
+		std::cout << "vertical_ray_angles: ";
+		for (const double a : vertical_ray_angles_) std::cout << a << " ";
+		std::cout << "\n";
+#endif
+
+		// Pass to radians:
+		for (double& a : vertical_ray_angles_) a = mrpt::DEG2RAD(a);
+	}
+
 	// 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 double largestVertFOV = std::max(
+		std::abs(vertical_ray_angles_.front()),
+		std::abs(vertical_ray_angles_.back()));
+
+	const double tanFOVhalf = ::tan(largestVertFOV);
 	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 1
+	camModel.asYAML().printAsYAML();
+#endif
+
 	if (!fbo_renderer_depth_)
 	{
 		mrpt::opengl::CFBORender::Parameters p;
@@ -329,9 +378,7 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene)
 			{
 				auto& entry = lut_[i].column[j];
 
-				const auto vertAng =
-					-vertical_fov_ * 0.5 + j * vertical_fov_ / (nRows - 1);
-
+				const double vertAng = vertical_ray_angles_.at(j);
 				const double cosVertAng = std::cos(vertAng);
 
 				const auto pixel_v = mrpt::round(
@@ -351,7 +398,7 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene)
 	else
 	{
 		// check:
-		ASSERT_EQUAL_(lut_.size(), numHorzRaysPerRender);
+		ASSERT_EQUAL_(lut_.size(), static_cast<size_t>(numHorzRaysPerRender));
 		ASSERT_EQUAL_(lut_.at(0).column.size(), nRows);
 	}
 
@@ -387,7 +434,6 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene)
 	auto& depth_log2lin = depth_log2lin_t::Instance();
 	const auto& depth_log2lin_lut =
 		depth_log2lin.lut_from_zn_zf(minRange_, maxRange_);
-
 #endif
 
 	for (size_t renderIdx = 0; renderIdx < numRenders; renderIdx++)

mvsim_tutorial/definitions/helios-32.sensor.xml → mvsim_tutorial/definitions/helios-32-FOV-31.sensor.xml

@@ -1,8 +1,13 @@
 <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>
+    <!-- vert_fov_degrees: If defined, a symmetric vertical FOV is used.
+         The alternative is using a custom list of angles in "vertical_ray_angles"
+    -->
+    <!-- <vert_fov_degrees>${vert_fov_degrees|70}</vert_fov_degrees> -->
+
+    <vert_nrays>32</vert_nrays>
+    <vertical_ray_angles>15 14 13 12 11 10 9 8 7 6 5 4 3 2 1 0 -1 -2 -3 -4 -5 -6 -7 -8 -9 -10 -11 -12 -13 -14 -15 -16</vertical_ray_angles>
 
     <!-- Horizontal / azimuth angular resolution:
         The rotation of the Helios sensor configurable: 5 / 10 / 20 Hz

mvsim_tutorial/definitions/helios-32-FOV-70.sensor.xml

@@ -0,0 +1,36 @@
+<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: If defined, a symmetric vertical FOV is used.
+         The alternative is using a custom list of angles in "vertical_ray_angles"
+    -->
+    <!-- <vert_fov_degrees>${vert_fov_degrees|70}</vert_fov_degrees> -->
+
+    <vert_nrays>32</vert_nrays>
+    <vertical_ray_angles>-55.020 -52.081 -49.142 -46.204 -43.265 -40.326 -37.387 -34.449 -31.510 -28.571 -25.632 -22.694 -19.755 -16.816 -13.877 -10.939 -8.000 -6.667 -5.333 -4.000 -2.667 -1.333  0.000 1.333 2.667 4.000 5.333 6.667 8.754 10.841 12.929 15</vertical_ray_angles>
+
+    <!-- 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/rplidar-a2.sensor.xml

@@ -19,7 +19,7 @@
     <raytrace_3d>${raytrace_3d|false}</raytrace_3d>
     <max_range>${max_range|16.0}</max_range>
 
-    <visual> 
+    <visual>
         <model_uri>https://mrpt.github.io/mvsim-models/turtlebot3.zip/turtlebot3/lds.dae</model_uri>
         <model_roll>90</model_roll>
         <model_scale>0.001</model_scale>

mvsim_tutorial/demo_warehouse.world.xml

@@ -113,16 +113,14 @@
 		  sensor_name="scanner1"
 		/>
 
-		<!-- velodyne-vlp16.sensor.xml -->
-		<!-- helios-32.sensor.xml -->
-		<include file="definitions/helios-32.sensor.xml"
+		<include file="definitions/helios-32-FOV-31.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"
+		  sensor_rate="5.0"
+		  horz_resolution_factor="1.0"
+		  vert_resolution_factor="1.0"
 		/>
 
 		<include file="definitions/camera.sensor.xml"