BUGFIX: In voxelmaps with non-identity sensorPose

MRPT · Oct 24, 2023 · 814650b · 814650b
1 parent 67358e9
commit 814650b
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 13 deletions.
diff --git 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
@@ -150,12 +150,12 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase<voxel_node_t>,
 
 	void insertPointCloudAsRays(
 		const mrpt::maps::CPointsMap& pts, const mrpt::math::TPoint3D& sensorPt,
-		const std::optional<const mrpt::poses::CPose3D>& robotPose =
+		const std::optional<const mrpt::poses::CPose3D>& sensorPose =
 			std::nullopt);
 
 	void insertPointCloudAsEndPoints(
 		const mrpt::maps::CPointsMap& pts, const mrpt::math::TPoint3D& sensorPt,
-		const std::optional<const mrpt::poses::CPose3D>& robotPose =
+		const std::optional<const mrpt::poses::CPose3D>& sensorPose =
 			std::nullopt);
 
 	/** Returns all occupied voxels as a point cloud. The shared_ptr is
@@ -480,7 +480,7 @@ bool CVoxelMapOccupancyBase<voxel_node_t, occupancy_t>::getPointOccupancy(
 template <typename voxel_node_t, typename occupancy_t>
 void CVoxelMapOccupancyBase<voxel_node_t, occupancy_t>::insertPointCloudAsRays(
 	const mrpt::maps::CPointsMap& pts, const mrpt::math::TPoint3D& sensorPt,
-	const std::optional<const mrpt::poses::CPose3D>& robotPose)
+	const std::optional<const mrpt::poses::CPose3D>& sensorPose)
 {
 	markAsChanged();
 
@@ -510,8 +510,9 @@ void CVoxelMapOccupancyBase<voxel_node_t, occupancy_t>::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 <typename voxel_node_t, typename occupancy_t>
 void CVoxelMapOccupancyBase<voxel_node_t, occupancy_t>::
 	insertPointCloudAsEndPoints(
 		const mrpt::maps::CPointsMap& pts, const mrpt::math::TPoint3D& sensorPt,
-		const std::optional<const mrpt::poses::CPose3D>& robotPose)
+		const std::optional<const mrpt::poses::CPose3D>& sensorPose)
 {
 	markAsChanged();
 
@@ -594,8 +595,9 @@ void CVoxelMapOccupancyBase<voxel_node_t, occupancy_t>::
 
 	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
@@ -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;
 }