diff --git a/definitions/helios-32-FOV-26.sensor.xml b/definitions/helios-32-FOV-26.sensor.xml
index 5529ee44..20996831 100644
--- a/definitions/helios-32-FOV-26.sensor.xml
+++ b/definitions/helios-32-FOV-26.sensor.xml
@@ -20,9 +20,13 @@
$f{(1.0/${sensor_rate|10})/55.296e-6}
- ${horz_resolution_factor|1.5}
+ ${horz_resolution_factor|1.0}
${vert_resolution_factor|1.0}
+
+ ${max_vert_relative_depth_to_interpolatate|0.3}
+ ${max_horz_relative_depth_to_interpolatate|0.1}
+
${sensor_std_noise|0.005}
${min_range|0.20}
${max_range|110.0}
diff --git a/definitions/helios-32-FOV-31.sensor.xml b/definitions/helios-32-FOV-31.sensor.xml
index 48f9a6ad..2a4a6799 100644
--- a/definitions/helios-32-FOV-31.sensor.xml
+++ b/definitions/helios-32-FOV-31.sensor.xml
@@ -20,9 +20,13 @@
$f{(1.0/${sensor_rate|10})/55.296e-6}
- ${horz_resolution_factor|1.5}
+ ${horz_resolution_factor|1.0}
${vert_resolution_factor|1.0}
+
+ ${max_vert_relative_depth_to_interpolatate|0.3}
+ ${max_horz_relative_depth_to_interpolatate|0.1}
+
${sensor_std_noise|0.005}
${min_range|0.20}
${max_range|110.0}
diff --git a/definitions/helios-32-FOV-70.sensor.xml b/definitions/helios-32-FOV-70.sensor.xml
index f72c0a34..7735dad6 100644
--- a/definitions/helios-32-FOV-70.sensor.xml
+++ b/definitions/helios-32-FOV-70.sensor.xml
@@ -20,9 +20,13 @@
$f{(1.0/${sensor_rate|10})/55.296e-6}
- ${horz_resolution_factor|1.5}
+ ${horz_resolution_factor|1.0}
${vert_resolution_factor|1.0}
+
+ ${max_vert_relative_depth_to_interpolatate|0.3}
+ ${max_horz_relative_depth_to_interpolatate|0.1}
+
${sensor_std_noise|0.005}
${min_range|0.20}
${max_range|110.0}
diff --git a/definitions/jackal.vehicle.xml b/definitions/jackal.vehicle.xml
index 14858b67..0031f8b6 100644
--- a/definitions/jackal.vehicle.xml
+++ b/definitions/jackal.vehicle.xml
@@ -59,10 +59,10 @@
diff --git a/definitions/ouster-os1.sensor.xml b/definitions/ouster-os1.sensor.xml
index 63109769..3595c1c7 100644
--- a/definitions/ouster-os1.sensor.xml
+++ b/definitions/ouster-os1.sensor.xml
@@ -21,9 +21,13 @@
${sensor_horz_nrays|1024}
- ${horz_resolution_factor|1.5}
+ ${horz_resolution_factor|1.0}
${vert_resolution_factor|1.0}
+
+ ${max_vert_relative_depth_to_interpolatate|0.3}
+ ${max_horz_relative_depth_to_interpolatate|0.1}
+
${sensor_std_noise|0.005}
${min_range|0.5}
${max_range|90.0}
diff --git a/definitions/velodyne-vlp16.sensor.xml b/definitions/velodyne-vlp16.sensor.xml
index f6910997..104c2c8d 100644
--- a/definitions/velodyne-vlp16.sensor.xml
+++ b/definitions/velodyne-vlp16.sensor.xml
@@ -17,9 +17,13 @@
$f{(60.0/${sensor_rpm|600})/55.296e-6}
- ${horz_resolution_factor|1.5}
+ ${horz_resolution_factor|1.0}
${vert_resolution_factor|1.0}
+
+ ${max_vert_relative_depth_to_interpolatate|0.3}
+ ${max_horz_relative_depth_to_interpolatate|0.1}
+
${sensor_std_noise|0.005}
${max_range|80.0}
diff --git a/modules/simulator/include/mvsim/Sensors/Lidar3D.h b/modules/simulator/include/mvsim/Sensors/Lidar3D.h
index d41b07f2..46c1fbe6 100644
--- a/modules/simulator/include/mvsim/Sensors/Lidar3D.h
+++ b/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
diff --git a/modules/simulator/src/Sensors/Lidar3D.cpp b/modules/simulator/src/Sensors/Lidar3D.cpp
index de94ebf1..b2da05d2 100644
--- a/modules/simulator/src/Sensors/Lidar3D.cpp
+++ b/modules/simulator/src/Sensors/Lidar3D.cpp
@@ -67,6 +67,11 @@ void Lidar3D::loadConfigFrom(const rapidxml::xml_node* 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;
+#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(u);
+ const int v0 = static_cast(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(
- mrpt::round(camModel.cx() - camModel.fx() * std::tan(horzAng)),
- 0, camModel.ncols - 1);
+ const auto pixel_u = mrpt::saturate_val(
+ 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(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;
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
{
diff --git a/mvsim_tutorial/demo_outdoor.world.xml b/mvsim_tutorial/demo_outdoor.world.xml
index 5c49e7ab..fc6c98dd 100644
--- a/mvsim_tutorial/demo_outdoor.world.xml
+++ b/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"
/>