3D LIDARS: Bilinear interpolation (when it makes sense) to obtain muc…

…h smoother point cloud simulations

definitions/helios-32-FOV-26.sensor.xml

@@ -20,9 +20,13 @@
     <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|1.5}</horz_resolution_factor>
+    <horz_resolution_factor>${horz_resolution_factor|1.0}</horz_resolution_factor>
     <vert_resolution_factor>${vert_resolution_factor|1.0}</vert_resolution_factor>
 
+    <!-- Depth ratio (0 to 1) between adjacent depth image to allow linear interpolation of ranges -->
+    <max_vert_relative_depth_to_interpolatate>${max_vert_relative_depth_to_interpolatate|0.3}</max_vert_relative_depth_to_interpolatate>
+    <max_horz_relative_depth_to_interpolatate>${max_horz_relative_depth_to_interpolatate|0.1}</max_horz_relative_depth_to_interpolatate>
+
     <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>

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

@@ -20,9 +20,13 @@
     <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|1.5}</horz_resolution_factor>
+    <horz_resolution_factor>${horz_resolution_factor|1.0}</horz_resolution_factor>
     <vert_resolution_factor>${vert_resolution_factor|1.0}</vert_resolution_factor>
 
+    <!-- Depth ratio (0 to 1) between adjacent depth image to allow linear interpolation of ranges -->
+    <max_vert_relative_depth_to_interpolatate>${max_vert_relative_depth_to_interpolatate|0.3}</max_vert_relative_depth_to_interpolatate>
+    <max_horz_relative_depth_to_interpolatate>${max_horz_relative_depth_to_interpolatate|0.1}</max_horz_relative_depth_to_interpolatate>
+
     <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>

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

@@ -20,9 +20,13 @@
     <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|1.5}</horz_resolution_factor>
+    <horz_resolution_factor>${horz_resolution_factor|1.0}</horz_resolution_factor>
     <vert_resolution_factor>${vert_resolution_factor|1.0}</vert_resolution_factor>
 
+    <!-- Depth ratio (0 to 1) between adjacent depth image to allow linear interpolation of ranges -->
+    <max_vert_relative_depth_to_interpolatate>${max_vert_relative_depth_to_interpolatate|0.3}</max_vert_relative_depth_to_interpolatate>
+    <max_horz_relative_depth_to_interpolatate>${max_horz_relative_depth_to_interpolatate|0.1}</max_horz_relative_depth_to_interpolatate>
+
     <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>

definitions/jackal.vehicle.xml

@@ -59,10 +59,10 @@
 
       <include file="helios-32-FOV-31.sensor.xml"
         sensor_x="0.15" sensor_z="$f{0.27 + 0.08}"
-        sensor_std_noise="0.005"
+        sensor_std_noise="0.0025"
         sensor_publish="false"
         sensor_name="lidar1"
-        sensor_rate="5.0"
+        sensor_rate="10.0"
         horz_resolution_factor="1.5"
         vert_resolution_factor="1.0"
       />

definitions/ouster-os1.sensor.xml

@@ -21,9 +21,13 @@
     <horz_nrays>${sensor_horz_nrays|1024}</horz_nrays>
 
     <!-- 1.0=minimum (faster), larger values=potentially finer details captured -->
-    <horz_resolution_factor>${horz_resolution_factor|1.5}</horz_resolution_factor>
+    <horz_resolution_factor>${horz_resolution_factor|1.0}</horz_resolution_factor>
     <vert_resolution_factor>${vert_resolution_factor|1.0}</vert_resolution_factor>
 
+    <!-- Depth ratio (0 to 1) between adjacent depth image to allow linear interpolation of ranges -->
+    <max_vert_relative_depth_to_interpolatate>${max_vert_relative_depth_to_interpolatate|0.3}</max_vert_relative_depth_to_interpolatate>
+    <max_horz_relative_depth_to_interpolatate>${max_horz_relative_depth_to_interpolatate|0.1}</max_horz_relative_depth_to_interpolatate>
+
     <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>

definitions/velodyne-vlp16.sensor.xml

@@ -17,9 +17,13 @@
     <horz_nrays>$f{(60.0/${sensor_rpm|600})/55.296e-6}</horz_nrays>
 
     <!-- 1.0=minimum (faster), larger values=potentially finer details captured -->
-    <horz_resolution_factor>${horz_resolution_factor|1.5}</horz_resolution_factor>
+    <horz_resolution_factor>${horz_resolution_factor|1.0}</horz_resolution_factor>
     <vert_resolution_factor>${vert_resolution_factor|1.0}</vert_resolution_factor>
 
+    <!-- Depth ratio (0 to 1) between adjacent depth image to allow linear interpolation of ranges -->
+    <max_vert_relative_depth_to_interpolatate>${max_vert_relative_depth_to_interpolatate|0.3}</max_vert_relative_depth_to_interpolatate>
+    <max_horz_relative_depth_to_interpolatate>${max_horz_relative_depth_to_interpolatate|0.1}</max_horz_relative_depth_to_interpolatate>
+
     <range_std_noise>${sensor_std_noise|0.005}</range_std_noise>
     <max_range>${max_range|80.0}</max_range>
 

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

@@ -81,6 +81,8 @@ class Lidar3D : public SensorBase
 	int vertNumRays_ = 16, horzNumRays_ = 180;
 	double horzResolutionFactor_ = 1.0;
 	double vertResolutionFactor_ = 1.0;
+	float maxDepthInterpolationStepVert_ = 0.30f;
+	float maxDepthInterpolationStepHorz_ = 0.10f;
 
 	/** Last simulated scan */
 	mrpt::obs::CObservationPointCloud::Ptr last_scan2gui_, last_scan_;
@@ -102,7 +104,7 @@ class Lidar3D : public SensorBase
 
 	struct PerRayLUT
 	{
-		int u = 0, v = 0;  //!< Pixel coords
+		float u = 0, v = 0;	 //!< Pixel coords
 		float depth2range = 0;
 	};
 	struct PerHorzAngleLUT

modules/simulator/src/Sensors/Lidar3D.cpp

@@ -67,6 +67,11 @@ void Lidar3D::loadConfigFrom(const rapidxml::xml_node<char>* root)
 	params["vert_resolution_factor"] =
 		TParamEntry("%lf", &vertResolutionFactor_);
 
+	params["max_vert_relative_depth_to_interpolatate"] =
+		TParamEntry("%f", &maxDepthInterpolationStepVert_);
+	params["max_horz_relative_depth_to_interpolatate"] =
+		TParamEntry("%f", &maxDepthInterpolationStepHorz_);
+
 	// Parse XML params:
 	parse_xmlnode_children_as_param(*root, params, varValues_);
 }
@@ -191,6 +196,102 @@ void Lidar3D::freeOpenGLResources()
 	fbo_renderer_depth_.reset();
 }
 
+#if MRPT_VERSION >= 0x270
+// Do the log->linear conversion ourselves for this sensor,
+// since only a few depth points are actually used:
+// (older mrpt versions already returned the linearized depth)
+constexpr int DEPTH_LOG2LIN_BITS = 20;
+using depth_log2lin_t =
+	mrpt::opengl::OpenGLDepth2LinearLUTs<DEPTH_LOG2LIN_BITS>;
+#endif
+
+static float safeInterpolateRangeImage(
+	const mrpt::math::CMatrixFloat& depthImage,
+	const float maxDepthInterpolationStepVert,
+	const float maxDepthInterpolationStepHorz, const int NCOLS, const int NROWS,
+	float v, float u
+#if MRPT_VERSION >= 0x270
+	,
+	const depth_log2lin_t::lut_t& depth_log2lin_lut
+#endif
+)
+{
+	const int u0 = static_cast<int>(u);
+	const int v0 = static_cast<int>(v);
+	const int u1 = std::min(u0 + 1, NCOLS - 1);
+	const int v1 = std::min(v0 + 1, NROWS - 1);
+
+	const float uw = u - u0;
+	const float vw = v - v0;
+
+	const float raw_d00 = depthImage(v0, u0);
+	const float raw_d01 = depthImage(v1, u0);
+	const float raw_d10 = depthImage(v0, u1);
+	const float raw_d11 = depthImage(v1, u1);
+
+	// Linearize:
+#if MRPT_VERSION >= 0x270
+	// Do the log->linear conversion ourselves for this sensor,
+	// since only a few depth points are actually used:
+
+	// map d in [-1.0f,+1.0f] ==> real depth values:
+	const float d00 = depth_log2lin_lut
+		[(raw_d00 + 1.0f) * (depth_log2lin_t::NUM_ENTRIES - 1) / 2];
+	const float d01 = depth_log2lin_lut
+		[(raw_d01 + 1.0f) * (depth_log2lin_t::NUM_ENTRIES - 1) / 2];
+	const float d10 = depth_log2lin_lut
+		[(raw_d10 + 1.0f) * (depth_log2lin_t::NUM_ENTRIES - 1) / 2];
+	const float d11 = depth_log2lin_lut
+		[(raw_d11 + 1.0f) * (depth_log2lin_t::NUM_ENTRIES - 1) / 2];
+#else
+	// "d" is already linear depth
+	const float d00 = raw_d00;
+	const float d01 = raw_d01;
+	const float d10 = raw_d10;
+	const float d11 = raw_d11;
+#endif
+
+	// max relative range difference in u and v directions:
+	const float A_u = std::max(std::abs(d00 - d10), std::abs(d01 - d11));
+	const float A_v = std::max(std::abs(d00 - d01), std::abs(d10 - d11));
+
+	const auto maxStepU = maxDepthInterpolationStepHorz * d00;
+	const auto maxStepV = maxDepthInterpolationStepVert * d00;
+
+	if (A_v < maxStepV && A_u < maxStepU)
+	{
+		// smooth bilinear interpolation in (u,v):
+		return d00 * (1.0f - uw) * (1.0f - vw) +  //
+			   d01 * (1.0f - uw) * vw +	 //
+			   d10 * uw * (1.0f - vw) +	 //
+			   d11 * uw * vw;
+	}
+	else if (A_v < maxStepV)
+	{
+		// Linear interpolation in "v" only:
+		// Pick closest "u":
+		const float d0 = uw < 0.5f ? d00 : d10;
+		const float d1 = uw < 0.5f ? d01 : d11;
+
+		return d0 * (1.0f - vw) + d1 * vw;
+	}
+	else if (A_u < maxStepU)
+	{
+		// Linear interpolation in "u" only:
+
+		// Pick closest "v":
+		const float d0 = vw < 0.5f ? d00 : d01;
+		const float d1 = vw < 0.5f ? d10 : d11;
+
+		return d0 * (1.0f - uw) + d1 * uw;
+	}
+	else
+	{
+		// too many changes in depth, do not interpolate:
+		return d00;
+	}
+}
+
 void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene)
 {
 	using namespace mrpt;  // _deg
@@ -374,9 +475,9 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene)
 
 			const double cosHorzAng = std::cos(horzAng);
 
-			const auto pixel_u = mrpt::saturate_val<int>(
-				mrpt::round(camModel.cx() - camModel.fx() * std::tan(horzAng)),
-				0, camModel.ncols - 1);
+			const auto pixel_u = mrpt::saturate_val<float>(
+				camModel.cx() - camModel.fx() * std::tan(horzAng), 0,
+				camModel.ncols - 1);
 
 			for (size_t j = 0; j < nRows; j++)
 			{
@@ -385,9 +486,9 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene)
 				const double vertAng = vertical_ray_angles_.at(j);
 				const double cosVertAng = std::cos(vertAng);
 
-				const auto pixel_v = mrpt::round(
-					camModel.cy() -
-					camModel.fy() * std::tan(vertAng) / cosHorzAng);
+				const auto pixel_v = camModel.cy() - camModel.fy() *
+														 std::tan(vertAng) /
+														 cosHorzAng;
 
 				// out of the simulated camera (should not happen?)
 				if (pixel_v < 0 || pixel_v >= static_cast<int>(camModel.nrows))
@@ -432,9 +533,6 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene)
 #if MRPT_VERSION >= 0x270
 	// Do the log->linear conversion ourselves for this sensor,
 	// since only a few depth points are actually used:
-	constexpr int DEPTH_LOG2LIN_BITS = 18;
-	using depth_log2lin_t =
-		mrpt::opengl::OpenGLDepth2LinearLUTs<DEPTH_LOG2LIN_BITS>;
 	auto& depth_log2lin = depth_log2lin_t::Instance();
 	const auto& depth_log2lin_lut =
 		depth_log2lin.lut_from_zn_zf(minRange_, maxRange_);
@@ -505,19 +603,15 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene)
 				ASSERTDEB_LT_(v, depthImage.rows());
 
 				// Depth:
-				float d = depthImage(v, u);
-
-				// Linearize:
+				float d = safeInterpolateRangeImage(
+					depthImage, maxDepthInterpolationStepVert_,
+					maxDepthInterpolationStepHorz_, FBO_NCOLS, FBO_NROWS, v, u
 #if MRPT_VERSION >= 0x270
-				// Do the log->linear conversion ourselves for this sensor,
-				// since only a few depth points are actually used:
-
-				// map d in [-1.0f,+1.0f] ==> real depth values:
-				d = depth_log2lin_lut
-					[(d + 1.0f) * (depth_log2lin_t::NUM_ENTRIES - 1) / 2];
-#else
-				// "d" is already linear depth
+					,
+					depth_log2lin_lut
 #endif
+				);
+
 				// Add noise:
 				if (d != 0)	 // invalid range
 				{

mvsim_tutorial/demo_outdoor.world.xml

@@ -54,7 +54,6 @@
 		  sensor_x="0.3" sensor_y="0" sensor_z="2.4" sensor_yaw="0"
 		  sensor_publish="false"
 		  sensor_name="lidar1"
-		  sensor_horz_nrays="1024"
 		/>
 		<!-- vert_fov_degrees="90" -->
 	</vehicle>