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; +}