diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index b18e805c29..223e949033 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,7 +1,9 @@ \page changelog Change Log # Version 2.11.3: UNRELEASED -(none) +- Changes in libraries: + - \ref mrpt_maps_grp + - mrpt::maps::COccupancyGridMap3D::insertObservation() now also handles mrpt::obs::CObservationPointCloud # Version 2.11.2: Released Oct 25th, 2023 - Changes in libraries: diff --git a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h index 0f81e67f09..97ea8d061e 100644 --- a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h +++ b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h @@ -17,6 +17,11 @@ #include #include +namespace mrpt::obs +{ +class CObservationPointCloud; +} + namespace mrpt::maps { /** A 3D occupancy grid map with a regular, even distribution of voxels. @@ -80,6 +85,10 @@ class COccupancyGridMap3D const mrpt::obs::CObservation3DRangeScan& o, const mrpt::poses::CPose3D& robotPose); + void internal_insertObservationPointCloud( + const mrpt::obs::CObservationPointCloud& o, + const mrpt::poses::CPose3D& robotPose); + public: /** Constructor */ COccupancyGridMap3D( @@ -185,7 +194,8 @@ class COccupancyGridMap3D void insertPointCloud( const mrpt::math::TPoint3D& sensorCenter, const mrpt::maps::CPointsMap& pts, - const float maxValidRange = std::numeric_limits::max()); + const float maxValidRange = std::numeric_limits::max(), + const std::optional& robotPose = std::nullopt); /** \sa renderingOptions */ void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels& gl_obj) const; @@ -238,6 +248,11 @@ class COccupancyGridMap3D /** Decimation for insertPointCloud() or 2D range scans (Default: 1) */ uint16_t decimation{1}; + + /** If true, raytrace and fill empty cells. If false, only the final + * (end point) of each ray will be marked as occupied. + */ + bool raytraceEmptyCells = true; }; /** With this struct options are provided to the observation insertion diff --git a/libs/maps/src/maps/COccupancyGridMap3D_insert.cpp b/libs/maps/src/maps/COccupancyGridMap3D_insert.cpp index 015bd69e22..e1a967d474 100644 --- a/libs/maps/src/maps/COccupancyGridMap3D_insert.cpp +++ b/libs/maps/src/maps/COccupancyGridMap3D_insert.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include using namespace mrpt::maps; @@ -41,6 +42,12 @@ bool COccupancyGridMap3D::internal_insertObservation( this->internal_insertObservationScan3D(*o, robotPose3D); return true; } + if (auto* o = dynamic_cast(&obs); + o != nullptr) + { + this->internal_insertObservationPointCloud(*o, robotPose3D); + return true; + } return false; @@ -73,7 +80,7 @@ void COccupancyGridMap3D::internal_insertObservationScan2D( void COccupancyGridMap3D::insertPointCloud( const mrpt::math::TPoint3D& sensorPt, const mrpt::maps::CPointsMap& pts, - const float maxValidRange) + const float maxValidRange, const std::optional& robotPose) { MRPT_START @@ -85,7 +92,10 @@ void COccupancyGridMap3D::insertPointCloud( for (std::size_t idx = 0; idx < xs.size(); idx += insertionOptions.decimation) { - insertRay(sensorPt, mrpt::math::TPoint3D(xs[idx], ys[idx], zs[idx])); + auto pt = mrpt::math::TPoint3D(xs[idx], ys[idx], zs[idx]); + if (robotPose) pt = robotPose->composePoint(pt); + + insertRay(sensorPt, pt); } MRPT_END @@ -153,6 +163,13 @@ void COccupancyGridMap3D::insertRay( // Skip if totally out of bounds: if (m_grid.isOutOfBounds(cx, cy, cz)) return; + // The occupied cell at the end: + updateCell_fast_occupied( + trg_cx, trg_cy, trg_cz, logodd_observation_occupied, + logodd_thres_occupied); + + if (!insertionOptions.raytraceEmptyCells) return; // done! + // Use "fractional integers" to approximate float operations // during the ray tracing: const int Acx = trg_cx - cx; @@ -195,10 +212,23 @@ void COccupancyGridMap3D::insertRay( if (m_grid.isOutOfBounds(cx, cy, cz)) break; } - // And finally, the occupied cell at the end: - updateCell_fast_occupied( - trg_cx, trg_cy, trg_cz, logodd_observation_occupied, - logodd_thres_occupied); + MRPT_END +} + +void COccupancyGridMap3D::internal_insertObservationPointCloud( + const mrpt::obs::CObservationPointCloud& o, + const mrpt::poses::CPose3D& robotPose) +{ + MRPT_START + + if (!o.pointcloud) return; + + const auto sensorPose3D = robotPose + o.sensorPose; + + const auto sensorPt = mrpt::math::TPoint3D(sensorPose3D.asTPose()); + insertPointCloud( + sensorPt, *o.pointcloud, insertionOptions.maxDistanceInsertion, + sensorPose3D); MRPT_END }