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" />