diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 950f7a5417..f0b09c31b3 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -10,6 +10,7 @@ - Address the new field `variance` in `sensor_msgs/Range` (Closes [issue #1270](https://github.com/MRPT/mrpt/issues/1270)). - BUG FIXES: - Fix wrong rendering of all wxWidgets-based OpenGL windows when using Ubuntu's display settings to change UI to a size different than 100% (Fixes [issue #1114](https://github.com/MRPT/mrpt/issues/1114)). + - Fix ignored sensorPose of mrpt::obs::CObservationPointCloud while inserting them into voxel maps. # Version 2.11.1: Released Oct 23rd, 2023 - Changes in libraries: diff --git a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h index be31c0b747..f10a43d7de 100644 --- a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h +++ b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h @@ -150,12 +150,12 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase, void insertPointCloudAsRays( const mrpt::maps::CPointsMap& pts, const mrpt::math::TPoint3D& sensorPt, - const std::optional& robotPose = + const std::optional& sensorPose = std::nullopt); void insertPointCloudAsEndPoints( const mrpt::maps::CPointsMap& pts, const mrpt::math::TPoint3D& sensorPt, - const std::optional& robotPose = + const std::optional& sensorPose = std::nullopt); /** Returns all occupied voxels as a point cloud. The shared_ptr is @@ -480,7 +480,7 @@ bool CVoxelMapOccupancyBase::getPointOccupancy( template void CVoxelMapOccupancyBase::insertPointCloudAsRays( const mrpt::maps::CPointsMap& pts, const mrpt::math::TPoint3D& sensorPt, - const std::optional& robotPose) + const std::optional& sensorPose) { markAsChanged(); @@ -510,8 +510,9 @@ void CVoxelMapOccupancyBase::insertPointCloudAsRays( // for each ray: for (size_t i = 0; i < xs.size(); i += insertionOptions.decimation) { - const auto pt = robotPose - ? robotPose->composePoint(mrpt::math::TPoint3D(xs[i], ys[i], zs[i])) + const auto pt = sensorPose + ? sensorPose->composePoint( + mrpt::math::TPoint3D(xs[i], ys[i], zs[i])) : mrpt::math::TPoint3D(xs[i], ys[i], zs[i]); if (insertionOptions.max_range > 0 && @@ -577,7 +578,7 @@ template void CVoxelMapOccupancyBase:: insertPointCloudAsEndPoints( const mrpt::maps::CPointsMap& pts, const mrpt::math::TPoint3D& sensorPt, - const std::optional& robotPose) + const std::optional& sensorPose) { markAsChanged(); @@ -594,8 +595,9 @@ void CVoxelMapOccupancyBase:: for (size_t i = 0; i < xs.size(); i += insertionOptions.decimation) { - const auto pt = robotPose - ? robotPose->composePoint(mrpt::math::TPoint3D(xs[i], ys[i], zs[i])) + const auto pt = sensorPose + ? sensorPose->composePoint( + mrpt::math::TPoint3D(xs[i], ys[i], zs[i])) : mrpt::math::TPoint3D(xs[i], ys[i], zs[i]); if (insertionOptions.max_range > 0 && diff --git a/libs/maps/src/maps/CVoxelMap.cpp b/libs/maps/src/maps/CVoxelMap.cpp index bd4166c5b1..1a1472f836 100644 --- a/libs/maps/src/maps/CVoxelMap.cpp +++ b/libs/maps/src/maps/CVoxelMap.cpp @@ -116,18 +116,21 @@ bool CVoxelMap::internal_insertObservation_Pts( if (!obs.pointcloud || obs.pointcloud->empty()) return false; mrpt::math::TPoint3D sensorPt; - mrpt::poses::CPose3D localSensorPose; + mrpt::poses::CPose3D localSensorPose, globalSensorPose; obs.getSensorPose(localSensorPose); if (robotPose) // - sensorPt = (*robotPose + localSensorPose).translation(); + globalSensorPose = *robotPose + localSensorPose; else - sensorPt = localSensorPose.translation(); + globalSensorPose = localSensorPose; + + sensorPt = globalSensorPose.translation(); // Insert rays: if (insertionOptions.ray_trace_free_space) - insertPointCloudAsRays(*obs.pointcloud, sensorPt, robotPose); + insertPointCloudAsRays(*obs.pointcloud, sensorPt, globalSensorPose); else - insertPointCloudAsEndPoints(*obs.pointcloud, sensorPt, robotPose); + insertPointCloudAsEndPoints( + *obs.pointcloud, sensorPt, globalSensorPose); return true; }