From d168e100ce0d61412d1c6614ae599ebaab108787 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Sat, 14 Nov 2026 20:40:43 +0100 Subject: [PATCH] mola_metric_maps: apply the observation sensorPose on insertion Point clouds, Velodyne scans and precomputed 3D range scan points are in the sensor frame, as in MRPT, but most map classes placed them with the robot pose only, so a sensor mounted off the robot origin produced a shifted and rotated map. Likelihood evaluation already used the sensor pose, so both were inconsistent. - HashedVoxelPointCloud, SparseVoxelPointCloud, SparseTreesPointCloud, NDT, TSDF: compose with robotPose + sensorPose. - NDT, TSDF: use the sensor position as ray origin for every observation type, including 2D and range-image scans. - KeyframePointCloudMap: keyframe poses stay robot poses; the cloud is moved to the robot frame if the sensor is not at the robot origin. - New regression test for all these classes. Insertion of clouds with an identity sensorPose, as done by FilterMerge in mp2p_icp and mola_lidar_odometry pipelines, is unchanged. Fixes #279 --- agents.md | 10 +- .../mola_metric_maps/KeyframePointCloudMap.h | 7 +- .../include/mola_metric_maps/NDT.h | 9 +- .../include/mola_metric_maps/TSDF.h | 9 +- .../src/HashedVoxelPointCloud.cpp | 14 +- .../src/KeyframePointCloudMap.cpp | 29 ++- mola_metric_maps/src/NDT.cpp | 31 ++- .../src/SparseTreesPointCloud.cpp | 13 +- .../src/SparseVoxelPointCloud.cpp | 14 +- mola_metric_maps/src/TSDF.cpp | 25 +-- mola_metric_maps/tests/CMakeLists.txt | 7 + .../tests/test-sensor-pose-insertion.cpp | 193 ++++++++++++++++++ 12 files changed, 314 insertions(+), 47 deletions(-) create mode 100644 mola_metric_maps/tests/test-sensor-pose-insertion.cpp diff --git a/agents.md b/agents.md index 0ed80346..acb1e2dc 100644 --- a/agents.md +++ b/agents.md @@ -283,6 +283,13 @@ Classes registered by `src/register.cpp` (these are the names a YAML must use): `mola::SparseVoxelPointCloud`, `mola::SparseTreesPointCloud`, `mola::NDT`, `mola::TSDF`, `mola::KeyframePointCloudMap`, `mola::IncrementalPointCloud`. +- `insertObservation()` follows MRPT: points of point clouds, Velodyne scans and + `points3D_*` are in the **sensor** frame, placed at `robotPose + sensorPose`; + TSDF/NDT use the sensor position as ray origin. `KeyframePointCloudMap` keeps + KF poses as robot poses and moves the cloud to the robot frame instead. + Pipelines (`FilterMerge`) insert robot-frame clouds with an identity `sensorPose`. + Tests: `tests/test-sensor-pose-insertion.cpp`. + - `TSDF` (new 2026): voxel-hashed truncated signed distance field. Each measured point writes a signed distance **along its own ray** into the voxels of a narrow cylinder around it (`ray_tube_voxels` is the coverage knob, @@ -639,8 +646,7 @@ Classes registered by `src/register.cpp` (these are the names a YAML must use): `trySetCreationOptions(const TCreationOptions&)` (raises if refused). The other option groups are live references. `ViewDirectionFilter.None` is `None_` in Python. - `insertPointCloud()` builds a `CObservationPointCloud` with the points in the **robot** - frame and an identity `sensorPose`, which most classes here ignore anyway (TSDF/NDT use - the robot position as ray origin). + frame and an identity `sensorPose`, so TSDF/NDT use the robot position as ray origin. - After changing bindings, regenerate `mola_metric_maps/_core.pyi` with `scripts/generate_stubs.py`. - pip wheel `mola-metric-maps` (`packaging/pip/`, read its `README-dev.md`): a diff --git a/mola_metric_maps/include/mola_metric_maps/KeyframePointCloudMap.h b/mola_metric_maps/include/mola_metric_maps/KeyframePointCloudMap.h index 510a8064..3a6759da 100644 --- a/mola_metric_maps/include/mola_metric_maps/KeyframePointCloudMap.h +++ b/mola_metric_maps/include/mola_metric_maps/KeyframePointCloudMap.h @@ -51,9 +51,10 @@ namespace mola * * ## Input * The caller inserts `mrpt::obs::CObservationPointCloud` observations, with - * points already expressed in the vehicle `base_link` frame and already motion - * compensated (deskewed). This is typically produced by an `mp2p_icp::Generator` - * plus optional filter pipeline. + * points already motion compensated (deskewed). This is typically produced by + * an `mp2p_icp::Generator` plus optional filter pipeline. As in MRPT, points are + * in the sensor frame, placed on the vehicle by the observation `sensorPose`; + * keyframes store them in the vehicle `base_link` frame. * * ## Per-keyframe data * Each `KeyFrame` holds: diff --git a/mola_metric_maps/include/mola_metric_maps/NDT.h b/mola_metric_maps/include/mola_metric_maps/NDT.h index 5d48f0bc..fc90dae7 100644 --- a/mola_metric_maps/include/mola_metric_maps/NDT.h +++ b/mola_metric_maps/include/mola_metric_maps/NDT.h @@ -612,12 +612,13 @@ class NDT : public mrpt::maps::CMetricMap, const mrpt::poses::CPose3D& pc_in_map, const float* xs, const float* ys, const float* zs, const std::size_t num_pts) const; - /** - (xs,ys,zs): Sensed point local coordinates in the robot frame. - * - pc_in_map: SE(3) pose of the robot in the map frame. + /** - (xs,ys,zs): Sensed point local coordinates, in the frame given by pc_in_map. + * - pc_in_map: SE(3) pose of the frame of (xs,ys,zs) in the map frame. + * - sensorPt: sensor position in the map frame, the origin of the rays. */ void internal_insertPointCloud3D( - const mrpt::poses::CPose3D& pc_in_map, const float* xs, const float* ys, const float* zs, - const std::size_t num_pts); + const mrpt::poses::CPose3D& pc_in_map, const mrpt::math::TPoint3Df& sensorPt, const float* xs, + const float* ys, const float* zs, const std::size_t num_pts); // See docs in base class bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation& obs) const override; diff --git a/mola_metric_maps/include/mola_metric_maps/TSDF.h b/mola_metric_maps/include/mola_metric_maps/TSDF.h index 4e138de4..4f5e3d9d 100644 --- a/mola_metric_maps/include/mola_metric_maps/TSDF.h +++ b/mola_metric_maps/include/mola_metric_maps/TSDF.h @@ -524,12 +524,13 @@ class TSDF : public mrpt::maps::CMetricMap, /** Removes voxels far from the given sensor position. */ void pruneFarVoxels(const mrpt::math::TPoint3Df& sensorPt); - /** - (xs,ys,zs): sensed point coordinates in the sensor frame. - * - pc_in_map: SE(3) pose of the sensor in the map frame. + /** - (xs,ys,zs): sensed point coordinates, in the frame given by pc_in_map. + * - pc_in_map: SE(3) pose of the frame of (xs,ys,zs) in the map frame. + * - sensorPt: sensor position in the map frame, the origin of the rays. */ void internal_insertPointCloud3D( - const mrpt::poses::CPose3D& pc_in_map, const float* xs, const float* ys, const float* zs, - const std::size_t num_pts); + const mrpt::poses::CPose3D& pc_in_map, const mrpt::math::TPoint3Df& sensorPt, const float* xs, + const float* ys, const float* zs, const std::size_t num_pts); }; } // namespace mola diff --git a/mola_metric_maps/src/HashedVoxelPointCloud.cpp b/mola_metric_maps/src/HashedVoxelPointCloud.cpp index 72deed47..01d08d10 100644 --- a/mola_metric_maps/src/HashedVoxelPointCloud.cpp +++ b/mola_metric_maps/src/HashedVoxelPointCloud.cpp @@ -381,9 +381,13 @@ bool HashedVoxelPointCloud::internal_insertObservation( // Empty point set, or load from XYZ in observation: if (o.hasPoints3D) { + // points3D_* are in the sensor frame: + const auto sensorPose3D = robotPose3D + o.sensorPose; for (size_t i = 0; i < o.points3D_x.size(); i++) + { this->insertPoint( - robotPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]})); + sensorPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]})); + } return true; } @@ -413,10 +417,12 @@ bool HashedVoxelPointCloud::internal_insertObservation( // Automatically generate pointcloud if needed: if (!o.point_cloud.size()) const_cast(o).generatePointCloud(); + // point_cloud is in the sensor frame: + const auto sensorPose3D = robotPose3D + o.sensorPose; for (size_t i = 0; i < o.point_cloud.x.size(); i++) { insertPoint( - robotPose3D.composePoint({o.point_cloud.x[i], o.point_cloud.y[i], o.point_cloud.z[i]})); + sensorPose3D.composePoint({o.point_cloud.x[i], o.point_cloud.y[i], o.point_cloud.z[i]})); } return true; @@ -430,9 +436,11 @@ bool HashedVoxelPointCloud::internal_insertObservation( const auto& ys = o.pointcloud->getPointsBufferRef_y(); const auto& zs = o.pointcloud->getPointsBufferRef_z(); + // The cloud is in the sensor frame: + const auto sensorPose3D = robotPose3D + o.sensorPose; for (size_t i = 0; i < xs.size(); i++) { - insertPoint(robotPose3D.composePoint({xs[i], ys[i], zs[i]})); + insertPoint(sensorPose3D.composePoint({xs[i], ys[i], zs[i]})); } return true; diff --git a/mola_metric_maps/src/KeyframePointCloudMap.cpp b/mola_metric_maps/src/KeyframePointCloudMap.cpp index e8b4d865..dcbf6214 100644 --- a/mola_metric_maps/src/KeyframePointCloudMap.cpp +++ b/mola_metric_maps/src/KeyframePointCloudMap.cpp @@ -178,6 +178,33 @@ void rotateViewDirectionFieldsOrFallback(Pts& pts, const Pose& tf) } } #endif + +/** The cloud of `obs` in the robot frame. Keyframes keep their points in the + * robot frame, while the observation holds them in the sensor frame, so a + * transformed copy is returned if the sensor is not at the robot origin. */ +mrpt::maps::CPointsMap::Ptr cloudInRobotFrame(const mrpt::obs::CObservationPointCloud& obs) +{ + if (obs.sensorPose == mrpt::poses::CPose3D::Identity()) + { + return obs.pointcloud; + } + + // Same class as the input, to keep all its point fields: + auto out = std::dynamic_pointer_cast( + obs.pointcloud->GetRuntimeClass()->createObject()); + ASSERT_(out); + out->insertAnotherMap(obs.pointcloud.get(), obs.sensorPose); + + // Before MRPT 3.6.0, insertAnotherMap() copies view_{x,y,z} verbatim: +#if MRPT_VERSION < 0x030600 +#if defined(MOLA_MM_HAS_ROTATE_VIEW_HEADER) + rotateViewDirectionFieldsOrFallback(*out, obs.sensorPose); +#else + rotateViewDirectionFieldsLegacy(*out, obs.sensorPose); +#endif +#endif + return out; +} } // namespace const thread_local auto ENV_DO_PROFILE_COV = @@ -2849,7 +2876,7 @@ bool KeyframePointCloudMap::internal_insertObservation( new_kf.timestamp = obs.timestamp; new_kf.pose(pc_in_map); - new_kf.pointcloud(obsPC->pointcloud); + new_kf.pointcloud(cloudInRobotFrame(*obsPC)); new_kf.buildCache(); cached_.reset(); diff --git a/mola_metric_maps/src/NDT.cpp b/mola_metric_maps/src/NDT.cpp index 7694095e..84b8c043 100644 --- a/mola_metric_maps/src/NDT.cpp +++ b/mola_metric_maps/src/NDT.cpp @@ -440,7 +440,10 @@ bool NDT::internal_insertObservation( { // Default values are (0,0,0) } - const auto sensorPt = robotPose3D.translation().cast(); + + // Rays start at the sensor, not at the robot origin: + const auto sensorPose3D = robotPose3D + obs.getSensorPose(); + const auto sensorPt = sensorPose3D.translation().cast(); if (insertionOptions.remove_voxels_farther_than > 0) { @@ -487,7 +490,8 @@ bool NDT::internal_insertObservation( const auto& ys = scanPoints->getPointsBufferRef_y(); const auto& zs = scanPoints->getPointsBufferRef_z(); - internal_insertPointCloud3D(robotPose3D, xs.data(), ys.data(), zs.data(), xs.size()); + // Points are already in the robot frame: + internal_insertPointCloud3D(robotPose3D, sensorPt, xs.data(), ys.data(), zs.data(), xs.size()); return true; } @@ -505,10 +509,13 @@ bool NDT::internal_insertObservation( // Empty point set, or load from XYZ in observation: if (o.hasPoints3D) { + // points3D_* are in the sensor frame: for (size_t i = 0; i < o.points3D_x.size(); i++) + { this->insertPoint( - robotPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]}), + sensorPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]}), sensorPt); + } return true; } @@ -522,7 +529,9 @@ bool NDT::internal_insertObservation( const auto& ys = pointMap.getPointsBufferRef_y(); const auto& zs = pointMap.getPointsBufferRef_z(); - internal_insertPointCloud3D(robotPose3D, xs.data(), ys.data(), zs.data(), xs.size()); + // unprojectInto() already brings the points into the robot frame: + internal_insertPointCloud3D( + robotPose3D, sensorPt, xs.data(), ys.data(), zs.data(), xs.size()); return true; } @@ -543,10 +552,11 @@ bool NDT::internal_insertObservation( const_cast(o).generatePointCloud(); } + // point_cloud is in the sensor frame: for (size_t i = 0; i < o.point_cloud.x.size(); i++) { insertPoint( - robotPose3D.composePoint({o.point_cloud.x[i], o.point_cloud.y[i], o.point_cloud.z[i]}), + sensorPose3D.composePoint({o.point_cloud.x[i], o.point_cloud.y[i], o.point_cloud.z[i]}), sensorPt); } @@ -562,9 +572,10 @@ bool NDT::internal_insertObservation( const auto& ys = o.pointcloud->getPointsBufferRef_y(); const auto& zs = o.pointcloud->getPointsBufferRef_z(); + // The cloud is in the sensor frame: for (size_t i = 0; i < xs.size(); i++) { - insertPoint(robotPose3D.composePoint({xs[i], ys[i], zs[i]}), sensorPt); + insertPoint(sensorPose3D.composePoint({xs[i], ys[i], zs[i]}), sensorPt); } return true; @@ -1065,21 +1076,19 @@ void NDT::TRenderOptions::loadFromConfigFile( } void NDT::internal_insertPointCloud3D( - const mrpt::poses::CPose3D& pc_in_map, const float* xs, const float* ys, const float* zs, - const std::size_t num_pts) + const mrpt::poses::CPose3D& pc_in_map, const mrpt::math::TPoint3Df& sensorPt, const float* xs, + const float* ys, const float* zs, const std::size_t num_pts) { MRPT_TRY_START voxels_.reserve(voxels_.size() + num_pts); - const auto sensorPose = pc_in_map.translation().cast(); - for (std::size_t i = 0; i < num_pts; i++) { // Transform the point from the scan reference to its global 3D // position: const auto gPt = pc_in_map.composePoint({xs[i], ys[i], zs[i]}); - insertPoint(gPt, sensorPose); + insertPoint(gPt, sensorPt); } MRPT_TRY_END diff --git a/mola_metric_maps/src/SparseTreesPointCloud.cpp b/mola_metric_maps/src/SparseTreesPointCloud.cpp index 85c8e4c7..e929569b 100644 --- a/mola_metric_maps/src/SparseTreesPointCloud.cpp +++ b/mola_metric_maps/src/SparseTreesPointCloud.cpp @@ -365,10 +365,12 @@ bool SparseTreesPointCloud::internal_insertObservation( // Empty point set, or load from XYZ in observation: if (o.hasPoints3D) { + // points3D_* are in the sensor frame: + const auto sensorPose3D = robotPose3D + o.sensorPose; for (size_t i = 0; i < o.points3D_x.size(); i++) { this->insertPoint( - robotPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]})); + sensorPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]})); } return true; @@ -404,9 +406,10 @@ bool SparseTreesPointCloud::internal_insertObservation( const_cast(o).generatePointCloud(); } + // point_cloud is in the sensor frame: internal_insertPointCloud3D( - robotPose3D, o.point_cloud.x.data(), o.point_cloud.y.data(), o.point_cloud.z.data(), - o.point_cloud.x.size()); + robotPose3D + o.sensorPose, o.point_cloud.x.data(), o.point_cloud.y.data(), + o.point_cloud.z.data(), o.point_cloud.x.size()); return true; } @@ -420,7 +423,9 @@ bool SparseTreesPointCloud::internal_insertObservation( const auto& ys = o.pointcloud->getPointsBufferRef_y(); const auto& zs = o.pointcloud->getPointsBufferRef_z(); - internal_insertPointCloud3D(robotPose3D, xs.data(), ys.data(), zs.data(), xs.size()); + // The cloud is in the sensor frame: + internal_insertPointCloud3D( + robotPose3D + o.sensorPose, xs.data(), ys.data(), zs.data(), xs.size()); return true; } diff --git a/mola_metric_maps/src/SparseVoxelPointCloud.cpp b/mola_metric_maps/src/SparseVoxelPointCloud.cpp index c2530d88..2a23ea4b 100644 --- a/mola_metric_maps/src/SparseVoxelPointCloud.cpp +++ b/mola_metric_maps/src/SparseVoxelPointCloud.cpp @@ -411,9 +411,13 @@ bool SparseVoxelPointCloud::internal_insertObservation( // Empty point set, or load from XYZ in observation: if (o.hasPoints3D) { + // points3D_* are in the sensor frame: + const auto sensorPose3D = robotPose3D + o.sensorPose; for (size_t i = 0; i < o.points3D_x.size(); i++) + { this->insertPoint( - robotPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]})); + sensorPose3D.composePoint({o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]})); + } return true; } @@ -443,10 +447,12 @@ bool SparseVoxelPointCloud::internal_insertObservation( // Automatically generate pointcloud if needed: if (!o.point_cloud.size()) const_cast(o).generatePointCloud(); + // point_cloud is in the sensor frame: + const auto sensorPose3D = robotPose3D + o.sensorPose; for (size_t i = 0; i < o.point_cloud.x.size(); i++) { insertPoint( - robotPose3D.composePoint({o.point_cloud.x[i], o.point_cloud.y[i], o.point_cloud.z[i]})); + sensorPose3D.composePoint({o.point_cloud.x[i], o.point_cloud.y[i], o.point_cloud.z[i]})); } return true; @@ -460,9 +466,11 @@ bool SparseVoxelPointCloud::internal_insertObservation( const auto& ys = o.pointcloud->getPointsBufferRef_y(); const auto& zs = o.pointcloud->getPointsBufferRef_z(); + // The cloud is in the sensor frame: + const auto sensorPose3D = robotPose3D + o.sensorPose; for (size_t i = 0; i < xs.size(); i++) { - insertPoint(robotPose3D.composePoint({xs[i], ys[i], zs[i]})); + insertPoint(sensorPose3D.composePoint({xs[i], ys[i], zs[i]})); } return true; diff --git a/mola_metric_maps/src/TSDF.cpp b/mola_metric_maps/src/TSDF.cpp index 75d561f6..ccd3c076 100644 --- a/mola_metric_maps/src/TSDF.cpp +++ b/mola_metric_maps/src/TSDF.cpp @@ -709,11 +709,9 @@ void TSDF::pruneFarVoxels(const mrpt::math::TPoint3Df& sensorPt) } void TSDF::internal_insertPointCloud3D( - const mrpt::poses::CPose3D& pc_in_map, const float* xs, const float* ys, const float* zs, - const std::size_t num_pts) + const mrpt::poses::CPose3D& pc_in_map, const mrpt::math::TPoint3Df& sensorPt, const float* xs, + const float* ys, const float* zs, const std::size_t num_pts) { - const auto sensorPt = pc_in_map.translation().cast(); - const bool warmingUp = bootstrapping_ && insertionOptions.bootstrap_with_points; if (warmingUp) { @@ -775,6 +773,10 @@ bool TSDF::internal_insertObservation( pruneFarVoxels(robotPose3D.translation().cast()); + // Rays start at the sensor, not at the robot origin: + const auto sensorPose3D = robotPose3D + obs.getSensorPose(); + const auto sensorPt = sensorPose3D.translation().cast(); + if (IS_CLASS(obs, CObservationPointCloud)) { const auto& o = dynamic_cast(obs); @@ -787,9 +789,8 @@ bool TSDF::internal_insertObservation( const auto& ys = o.pointcloud->getPointsBufferRef_y(); const auto& zs = o.pointcloud->getPointsBufferRef_z(); - // The sensor pose on the robot is already applied by whoever built the - // cloud in the observation, as elsewhere in this library. - internal_insertPointCloud3D(robotPose3D, xs.data(), ys.data(), zs.data(), xs.size()); + // The cloud is in the sensor frame: + internal_insertPointCloud3D(sensorPose3D, sensorPt, xs.data(), ys.data(), zs.data(), xs.size()); return true; } @@ -807,7 +808,8 @@ bool TSDF::internal_insertObservation( const auto& ys = scanPoints->getPointsBufferRef_y(); const auto& zs = scanPoints->getPointsBufferRef_z(); - internal_insertPointCloud3D(robotPose3D, xs.data(), ys.data(), zs.data(), xs.size()); + // Points are already in the robot frame: + internal_insertPointCloud3D(robotPose3D, sensorPt, xs.data(), ys.data(), zs.data(), xs.size()); return true; } @@ -832,7 +834,7 @@ bool TSDF::internal_insertObservation( { pointMap.insertPointFast(o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]); } - cloudPose = robotPose3D + o.sensorPose; + cloudPose = sensorPose3D; } else if (o.hasRangeImage) { @@ -847,7 +849,7 @@ bool TSDF::internal_insertObservation( const auto& ys = pointMap.getPointsBufferRef_y(); const auto& zs = pointMap.getPointsBufferRef_z(); - internal_insertPointCloud3D(cloudPose, xs.data(), ys.data(), zs.data(), xs.size()); + internal_insertPointCloud3D(cloudPose, sensorPt, xs.data(), ys.data(), zs.data(), xs.size()); return true; } @@ -868,8 +870,7 @@ bool TSDF::internal_insertObservation( const auto& ys = pointMap.getPointsBufferRef_y(); const auto& zs = pointMap.getPointsBufferRef_z(); - internal_insertPointCloud3D( - robotPose3D + o.sensorPose, xs.data(), ys.data(), zs.data(), xs.size()); + internal_insertPointCloud3D(sensorPose3D, sensorPt, xs.data(), ys.data(), zs.data(), xs.size()); return true; } diff --git a/mola_metric_maps/tests/CMakeLists.txt b/mola_metric_maps/tests/CMakeLists.txt index d483447e..470c8faf 100644 --- a/mola_metric_maps/tests/CMakeLists.txt +++ b/mola_metric_maps/tests/CMakeLists.txt @@ -20,6 +20,13 @@ mola_add_test( mola_metric_maps ) +mola_add_test( + TARGET test-mola_metric_maps_sensor_pose_insertion + SOURCES test-sensor-pose-insertion.cpp + LINK_LIBRARIES + mola_metric_maps +) + mola_add_test( TARGET test-mola_metric_maps_hashedvoxels SOURCES test-mola_metric_maps_hashedvoxels.cpp diff --git a/mola_metric_maps/tests/test-sensor-pose-insertion.cpp b/mola_metric_maps/tests/test-sensor-pose-insertion.cpp new file mode 100644 index 00000000..379d9ce9 --- /dev/null +++ b/mola_metric_maps/tests/test-sensor-pose-insertion.cpp @@ -0,0 +1,193 @@ +/* _ + _ __ ___ ___ | | __ _ +| '_ ` _ \ / _ \| |/ _` | Modular Optimization framework for +| | | | | | (_) | | (_| | Localization and mApping (MOLA) +|_| |_| |_|\___/|_|\__,_| https://github.com/MOLAorg/mola + + Copyright (C) 2018-2026 Jose Luis Blanco, University of Almeria, + and individual contributors. + SPDX-License-Identifier: GPL-3.0 + See LICENSE for full license information. +*/ + +/** + * @file test-sensor-pose-insertion.cpp + * @brief insertObservation() places points given in the sensor frame at + * robotPose + sensorPose, as MRPT's own point maps do. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace +{ +using mrpt::DEG2RAD; + +const mrpt::poses::CPose3D robotPose(3.0, 2.0, 0.1, DEG2RAD(45.0), 0.0, 0.0); +const mrpt::poses::CPose3D sensorPose(1.0, -0.5, 0.8, DEG2RAD(30.0), DEG2RAD(10.0), DEG2RAD(-5.0)); + +/// A 4x2 m wall, 5 m in front of the sensor, in the sensor frame. +mrpt::maps::CSimplePointsMap::Ptr wallInSensorFrame() +{ + auto pts = mrpt::maps::CSimplePointsMap::Create(); + for (int i = -40; i <= 40; i++) + { + for (int j = -20; j <= 20; j++) + { + pts->insertPoint(5.0f, static_cast(i * 0.05), static_cast(j * 0.05)); + } + } + return pts; +} + +mrpt::obs::CObservationPointCloud::Ptr pointCloudObs() +{ + auto obs = mrpt::obs::CObservationPointCloud::Create(); + obs->pointcloud = wallInSensorFrame(); + obs->sensorPose = sensorPose; + return obs; +} + +/// The same wall, already transformed to the robot frame, with the sensor at +/// the robot origin. +mrpt::obs::CObservationPointCloud::Ptr referenceObs() +{ + const auto inSensor = wallInSensorFrame(); + auto inRobot = mrpt::maps::CSimplePointsMap::Create(); + for (size_t k = 0; k < inSensor->size(); k++) + { + mrpt::math::TPoint3Df p; + inSensor->getPoint(k, p.x, p.y, p.z); + inRobot->insertPoint(sensorPose.composePoint(p)); + } + auto obs = mrpt::obs::CObservationPointCloud::Create(); + obs->pointcloud = inRobot; + return obs; +} + +mrpt::obs::CObservation3DRangeScan::Ptr rangeScanObs() +{ + const auto pts = wallInSensorFrame(); + auto obs = mrpt::obs::CObservation3DRangeScan::Create(); + obs->hasPoints3D = true; + obs->resizePoints3DVectors(pts->size()); + for (size_t k = 0; k < pts->size(); k++) + { + pts->getPoint(k, obs->points3D_x[k], obs->points3D_y[k], obs->points3D_z[k]); + } + obs->sensorPose = sensorPose; + return obs; +} + +template +struct HasVisitAllPoints : std::false_type +{ +}; +template +struct HasVisitAllPoints< + MAP, std::void_t().visitAllPoints( + std::function()))>> : std::true_type +{ +}; + +/// Centroid of all the points in a map. +template +mrpt::math::TPoint3D mapCentroid(const MAP& map) +{ + mrpt::math::TPoint3D c(0, 0, 0); + size_t n = 0; + + const auto add = [&](const mrpt::math::TPoint3Df& p) + { + c += mrpt::math::TPoint3D(p); + n++; + }; + + if constexpr (HasVisitAllPoints::value) + { + map.visitAllPoints(add); + } + else + { + const auto* pts = map.getAsSimplePointsMap(); + ASSERT_(pts); + for (size_t k = 0; k < pts->size(); k++) + { + mrpt::math::TPoint3Df p; + pts->getPoint(k, p.x, p.y, p.z); + add(p); + } + } + ASSERT_(n > 0); + return c * (1.0 / static_cast(n)); +} + +template +void test_map(const mrpt::obs::CObservation& obs, const std::string& name) +{ + MAP map; + ASSERT_(map.insertObservation(obs, robotPose)); + + // Same points, given in the robot frame: + MAP ref; + ASSERT_(ref.insertObservation(*referenceObs(), robotPose)); + + const auto c = mapCentroid(map); + const auto expected = mapCentroid(ref); + const auto err = (c - expected).norm(); + + // The sensor offset is over 1 m, so ignoring it would give a much larger + // error. The tolerance covers TSDF, whose rays start at a different point in + // the reference: + ASSERTMSG_( + err < 0.05, mrpt::format( + "%s: map centroid %s, expected %s", name.c_str(), c.asString().c_str(), + expected.asString().c_str())); + + std::cout << "[Test Passed] " << name << " (err=" << err << " m)\n"; +} + +} // namespace + +int main() +{ + try + { + const auto pc = pointCloudObs(); + test_map(*pc, "HashedVoxelPointCloud, point cloud"); + test_map(*pc, "SparseVoxelPointCloud, point cloud"); + test_map(*pc, "SparseTreesPointCloud, point cloud"); + test_map(*pc, "NDT, point cloud"); + test_map(*pc, "TSDF, point cloud"); + test_map(*pc, "KeyframePointCloudMap, point cloud"); + + const auto rs = rangeScanObs(); + test_map(*rs, "HashedVoxelPointCloud, 3D range scan"); + test_map(*rs, "SparseVoxelPointCloud, 3D range scan"); + test_map(*rs, "SparseTreesPointCloud, 3D range scan"); + test_map(*rs, "NDT, 3D range scan"); + test_map(*rs, "TSDF, 3D range scan"); + } + catch (const std::exception& e) + { + std::cerr << mrpt::exception_to_str(e) << "\n"; + return 1; + } + return 0; +}