From c421202ba01bbc7a4ec5ec7747f2af82b3a083dd Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 25 Oct 2023 21:24:20 +0200 Subject: [PATCH 01/36] patch version bump --- appveyor.yml | 2 +- doc/source/doxygen-docs/changelog.md | 3 +++ package.xml | 2 +- version_prefix.txt | 2 +- 4 files changed, 6 insertions(+), 3 deletions(-) diff --git a/appveyor.yml b/appveyor.yml index 4099c44fc0..75ca0852f7 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -1,5 +1,5 @@ # version format -version: 2.11.2-{branch}-build{build} +version: 2.11.3-{branch}-build{build} os: Visual Studio 2019 diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 1a1d946d13..b18e805c29 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,5 +1,8 @@ \page changelog Change Log +# Version 2.11.3: UNRELEASED +(none) + # Version 2.11.2: Released Oct 25th, 2023 - Changes in libraries: - \ref mrpt_gui_grp diff --git a/package.xml b/package.xml index efb834030d..e636e9288d 100644 --- a/package.xml +++ b/package.xml @@ -7,7 +7,7 @@ mrpt2 - 2.11.2 + 2.11.3 Mobile Robot Programming Toolkit (MRPT) version 2.x Jose-Luis Blanco-Claraco diff --git a/version_prefix.txt b/version_prefix.txt index 348a6a3c77..94ecf1513d 100644 --- a/version_prefix.txt +++ b/version_prefix.txt @@ -1,4 +1,4 @@ -2.11.2 +2.11.3 # IMPORTANT: This file is parsed by CMake, don't add any comment to # the first line. # This file is used in both Windows and Linux scripts to automatically From 7245f45f3c4ce2689ddac77324622672faf0c39a Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Thu, 2 Nov 2023 23:40:59 +0100 Subject: [PATCH 02/36] More obs types handled in 3d grid map --- doc/source/doxygen-docs/changelog.md | 4 +- .../include/mrpt/maps/COccupancyGridMap3D.h | 17 +++++++- .../src/maps/COccupancyGridMap3D_insert.cpp | 42 ++++++++++++++++--- 3 files changed, 55 insertions(+), 8 deletions(-) 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 } From 1651dcb104e8be394b6717b15bb72fbf6c4f81b4 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 3 Nov 2023 02:05:50 +0100 Subject: [PATCH 03/36] fix typos ("outer") --- libs/math/include/mrpt/math/math_frwds.h | 2 +- libs/nav/include/mrpt/nav/reactive/TWaypoint.h | 6 +++--- libs/nav/src/reactive/TWaypoint.cpp | 10 +++++----- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/libs/math/include/mrpt/math/math_frwds.h b/libs/math/include/mrpt/math/math_frwds.h index 47dbe7b82f..2fa0adfa1e 100644 --- a/libs/math/include/mrpt/math/math_frwds.h +++ b/libs/math/include/mrpt/math/math_frwds.h @@ -24,7 +24,7 @@ template class Map; template class InnerStride; -template +template class Stride; template < typename _Scalar, int _Rows, int _Cols, int _Options, diff --git a/libs/nav/include/mrpt/nav/reactive/TWaypoint.h b/libs/nav/include/mrpt/nav/reactive/TWaypoint.h index 96719403f1..708bbf4af5 100644 --- a/libs/nav/include/mrpt/nav/reactive/TWaypoint.h +++ b/libs/nav/include/mrpt/nav/reactive/TWaypoint.h @@ -91,9 +91,9 @@ struct TWaypointsRenderingParams { TWaypointsRenderingParams(); - double outter_radius{.3}, inner_radius{.2}; - double outter_radius_non_skippable{.3}, inner_radius_non_skippable{.0}; - double outter_radius_reached{.2}, inner_radius_reached{.1}; + double outer_radius{.3}, inner_radius{.2}; + double outer_radius_non_skippable{.3}, inner_radius_non_skippable{.0}; + double outer_radius_reached{.2}, inner_radius_reached{.1}; double heading_arrow_len{1.0}; mrpt::img::TColor color_regular, color_current_goal, color_reached; bool show_labels{true}; diff --git a/libs/nav/src/reactive/TWaypoint.cpp b/libs/nav/src/reactive/TWaypoint.cpp index 12d607a33b..f50c8dbfe8 100644 --- a/libs/nav/src/reactive/TWaypoint.cpp +++ b/libs/nav/src/reactive/TWaypoint.cpp @@ -134,8 +134,8 @@ void TWaypointSequence::getAsOpenglVisualization( for (const auto& p : waypoints) { auto gl_pt = mrpt::opengl::CDisk::Create( - p.allow_skip ? params.outter_radius - : params.outter_radius_non_skippable, + p.allow_skip ? params.outer_radius + : params.outer_radius_non_skippable, p.allow_skip ? params.inner_radius : params.inner_radius_non_skippable, 15); @@ -172,9 +172,9 @@ void TWaypointStatusSequence::getAsOpenglVisualization( const bool is_cur_goal = (int(idx) == waypoint_index_current_goal); mrpt::opengl::CDisk::Ptr gl_pt = mrpt::opengl::CDisk::Create( - p.reached ? params.outter_radius_reached - : (p.allow_skip ? params.outter_radius - : params.outter_radius_non_skippable), + p.reached ? params.outer_radius_reached + : (p.allow_skip ? params.outer_radius + : params.outer_radius_non_skippable), p.reached ? params.inner_radius_reached : (p.allow_skip ? params.inner_radius : params.inner_radius_non_skippable), From d4f34306f3749f326c9fa1750069cee6fe1991da Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 3 Nov 2023 11:51:52 +0100 Subject: [PATCH 04/36] more typos --- apps/benchmarking-image-features/src/place_recognition.cpp | 4 ++-- python/src/mrpt/nav/reactive/TWaypoint.cpp | 6 +++--- python/stubs-out/mrpt/pymrpt/mrpt/nav.pyi | 6 +++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/apps/benchmarking-image-features/src/place_recognition.cpp b/apps/benchmarking-image-features/src/place_recognition.cpp index 78aac12663..6d6e6fe472 100644 --- a/apps/benchmarking-image-features/src/place_recognition.cpp +++ b/apps/benchmarking-image-features/src/place_recognition.cpp @@ -145,7 +145,7 @@ int PlaceRecognition::predictLabel2( } // iterates over each key-point in the training images / dataset - } // end of outter loop iterates over each key-point + } // end of outer loop iterates over each key-point for (int i = 0; i < feats_size; i++) cout << labels[i] << " "; @@ -207,7 +207,7 @@ int PlaceRecognition::predictLabel( } } } // iterates over each key-point in the training images / dataset - } // end of outter loop iterates over each key-point + } // end of outer loop iterates over each key-point for (int i = 0; i < feats_size; i++) cout << labels[i] << " "; diff --git a/python/src/mrpt/nav/reactive/TWaypoint.cpp b/python/src/mrpt/nav/reactive/TWaypoint.cpp index 5d8227e259..0a7062a655 100644 --- a/python/src/mrpt/nav/reactive/TWaypoint.cpp +++ b/python/src/mrpt/nav/reactive/TWaypoint.cpp @@ -425,11 +425,11 @@ void bind_mrpt_nav_reactive_TWaypoint(std::function< pybind11::module &(std::str pybind11::class_> cl(M("mrpt::nav"), "TWaypointsRenderingParams", "used in getAsOpenglVisualization() "); cl.def( pybind11::init( [](){ return new mrpt::nav::TWaypointsRenderingParams(); } ) ); cl.def( pybind11::init( [](mrpt::nav::TWaypointsRenderingParams const &o){ return new mrpt::nav::TWaypointsRenderingParams(o); } ) ); - cl.def_readwrite("outter_radius", &mrpt::nav::TWaypointsRenderingParams::outter_radius); + cl.def_readwrite("outer_radius", &mrpt::nav::TWaypointsRenderingParams::outer_radius); cl.def_readwrite("inner_radius", &mrpt::nav::TWaypointsRenderingParams::inner_radius); - cl.def_readwrite("outter_radius_non_skippable", &mrpt::nav::TWaypointsRenderingParams::outter_radius_non_skippable); + cl.def_readwrite("outer_radius_non_skippable", &mrpt::nav::TWaypointsRenderingParams::outer_radius_non_skippable); cl.def_readwrite("inner_radius_non_skippable", &mrpt::nav::TWaypointsRenderingParams::inner_radius_non_skippable); - cl.def_readwrite("outter_radius_reached", &mrpt::nav::TWaypointsRenderingParams::outter_radius_reached); + cl.def_readwrite("outer_radius_reached", &mrpt::nav::TWaypointsRenderingParams::outer_radius_reached); cl.def_readwrite("inner_radius_reached", &mrpt::nav::TWaypointsRenderingParams::inner_radius_reached); cl.def_readwrite("heading_arrow_len", &mrpt::nav::TWaypointsRenderingParams::heading_arrow_len); cl.def_readwrite("color_regular", &mrpt::nav::TWaypointsRenderingParams::color_regular); diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/nav.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/nav.pyi index c5042b3e43..d0fa18b568 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/nav.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/nav.pyi @@ -1994,9 +1994,9 @@ class TWaypointsRenderingParams: inner_radius: float inner_radius_non_skippable: float inner_radius_reached: float - outter_radius: float - outter_radius_non_skippable: float - outter_radius_reached: float + outer_radius: float + outer_radius_non_skippable: float + outer_radius_reached: float show_labels: bool @overload def __init__(self) -> None: ... From 2f71c2e69945c0ba08a287bfdb2d3bd76f7744b1 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sat, 4 Nov 2023 16:47:33 +0100 Subject: [PATCH 05/36] New NearestNeighborsCapable interface --- doc/source/doxygen-docs/changelog.md | 5 + .../include/mrpt/maps/COccupancyGridMap2D.h | 31 +++++- libs/maps/include/mrpt/maps/CPointsMap.h | 31 +++++- .../mrpt/maps/CVoxelMapOccupancyBase.h | 55 ++++++++++- .../mrpt/maps/NearestNeighborsCapable.h | 93 ++++++++++++++++++ .../src/maps/COccupancyGridMap2D_common.cpp | 65 ++++++++++++- .../src/maps/COccupancyGridMap2D_unittest.cpp | 16 +++- libs/maps/src/maps/CPointsMap.cpp | 94 +++++++++++++++++++ 8 files changed, 384 insertions(+), 6 deletions(-) create mode 100644 libs/maps/include/mrpt/maps/NearestNeighborsCapable.h diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 223e949033..b53b0675b0 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -4,6 +4,11 @@ - Changes in libraries: - \ref mrpt_maps_grp - mrpt::maps::COccupancyGridMap3D::insertObservation() now also handles mrpt::obs::CObservationPointCloud + - New virtual interface mrpt::maps::NearestNeighborsCapable, implemented in: + - All mrpt::maps::CPointsMap classes + - All classes derived from mrpt::maps::CVoxelMapOccupancyBase + - mrpt::maps::COccupancyGridMap2D + - mrpt::maps::COccupancyGridMap2D # Version 2.11.2: Released Oct 25th, 2023 - Changes in libraries: diff --git a/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h b/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h index d64a0e97c7..7713971815 100644 --- a/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h +++ b/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -52,7 +53,8 @@ namespace mrpt::maps **/ class COccupancyGridMap2D : public CMetricMap, - public CLogOddsGridMap2D + public CLogOddsGridMap2D, + public mrpt::maps::NearestNeighborsCapable { DEFINE_SERIALIZABLE(COccupancyGridMap2D, mrpt::maps) public: @@ -1161,6 +1163,33 @@ class COccupancyGridMap2D getXMin(), getYMin(), getXMax(), getYMax(), getResolution()); } + /** @name API of the NearestNeighborsCapable virtual interface + @{ */ + // See docs in base class + [[nodiscard]] bool nn_single_search( + const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, + float& out_dist_sqr) const override; + [[nodiscard]] bool nn_single_search( + const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, + float& out_dist_sqr) const override; + void nn_multiple_search( + const mrpt::math::TPoint3Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const override; + void nn_multiple_search( + const mrpt::math::TPoint2Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const override; + void nn_radius_search( + const mrpt::math::TPoint3Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const override; + void nn_radius_search( + const mrpt::math::TPoint2Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const override; + /** @} */ + private: // See docs in base class double internal_computeObservationLikelihood( diff --git a/libs/maps/include/mrpt/maps/CPointsMap.h b/libs/maps/include/mrpt/maps/CPointsMap.h index 85c747aa5c..656bf688ff 100644 --- a/libs/maps/include/mrpt/maps/CPointsMap.h +++ b/libs/maps/include/mrpt/maps/CPointsMap.h @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -70,7 +71,8 @@ struct pointmap_traits; class CPointsMap : public CMetricMap, public mrpt::math::KDTreeCapable, public mrpt::opengl::PLY_Importer, - public mrpt::opengl::PLY_Exporter + public mrpt::opengl::PLY_Exporter, + public mrpt::maps::NearestNeighborsCapable { DEFINE_VIRTUAL_SERIALIZABLE(CPointsMap) // This must be added for declaration of MEX-related functions @@ -1124,6 +1126,33 @@ class CPointsMap : public CMetricMap, boundingBox().asString().c_str()); } + /** @name API of the NearestNeighborsCapable virtual interface + @{ */ + // See docs in base class + [[nodiscard]] bool nn_single_search( + const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, + float& out_dist_sqr) const override; + [[nodiscard]] bool nn_single_search( + const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, + float& out_dist_sqr) const override; + void nn_multiple_search( + const mrpt::math::TPoint3Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const override; + void nn_multiple_search( + const mrpt::math::TPoint2Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const override; + void nn_radius_search( + const mrpt::math::TPoint3Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const override; + void nn_radius_search( + const mrpt::math::TPoint2Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const override; + /** @} */ + protected: /** The point coordinates */ mrpt::aligned_std_vector m_x, m_y, m_z; diff --git a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h index f10a43d7de..655c9e9a24 100644 --- a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h +++ b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -112,7 +113,8 @@ struct has_color> : std::true_type */ template class CVoxelMapOccupancyBase : public CVoxelMapBase, - public detail::logoddscell_traits + public detail::logoddscell_traits, + public mrpt::maps::NearestNeighborsCapable { protected: using occupancy_value_t = occupancy_t; @@ -286,6 +288,57 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase, return get_logodd_lut().p2l(p); } + /** @name API of the NearestNeighborsCapable virtual interface + @{ */ + // See docs in base class + [[nodiscard]] bool nn_single_search( + const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, + float& out_dist_sqr) const override + { + return getOccupiedVoxels()->nn_single_search( + query, result, out_dist_sqr); + } + [[nodiscard]] bool nn_single_search( + const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, + float& out_dist_sqr) const override + { + return getOccupiedVoxels()->nn_single_search( + query, result, out_dist_sqr); + } + void nn_multiple_search( + const mrpt::math::TPoint3Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const override + { + getOccupiedVoxels()->nn_multiple_search( + query, N, results, out_dists_sqr); + } + void nn_multiple_search( + const mrpt::math::TPoint2Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const override + { + getOccupiedVoxels()->nn_multiple_search( + query, N, results, out_dists_sqr); + } + void nn_radius_search( + const mrpt::math::TPoint3Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const override + { + getOccupiedVoxels()->nn_radius_search( + query, search_radius_sqr, results, out_dists_sqr); + } + void nn_radius_search( + const mrpt::math::TPoint2Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const override + { + getOccupiedVoxels()->nn_radius_search( + query, search_radius_sqr, results, out_dists_sqr); + } + /** @} */ + protected: void internal_clear() override; diff --git a/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h b/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h new file mode 100644 index 0000000000..98b4d953e3 --- /dev/null +++ b/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h @@ -0,0 +1,93 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include + +namespace mrpt::maps +{ +/** Virtual interface for maps having the capability of searching the closest + * neighbor(s) of a given query 2D or 3D point. + * + * Note this is more generic than mrpt::math::KDTreeCapable since it does not + * assume the use of KD-trees, and it is also non templatized, so users can use + * dynamic casting to interact with maps in a generic way. + * + * \note New in MRPT 2.11.3 + * \ingroup mrpt_maps_grp + */ +class NearestNeighborsCapable +{ + public: + NearestNeighborsCapable() = default; + virtual ~NearestNeighborsCapable() = default; + + /** @name API of the NearestNeighborsCapable virtual interface + @{ */ + + /** Search for the closest 3D point to a given one. + * + * \param[in] query The query input point. + * \param[out] result The found closest point. + * \param[out] out_dist_sqr The square Euclidean distance between the query + * and the returned point. + * + * \return True if successful, false if no point was found. + */ + [[nodiscard]] virtual bool nn_single_search( + const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, + float& out_dist_sqr) const = 0; + + /// \overload for 2D points + [[nodiscard]] virtual bool nn_single_search( + const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, + float& out_dist_sqr) const = 0; + + /** Search for the `N` closest 3D points to a given one. + * + * \param[in] query The query input point. + * \param[out] results The found closest points. + * \param[out] out_dists_sqr The square Euclidean distances between the + * query and the returned point. + */ + virtual void nn_multiple_search( + const mrpt::math::TPoint3Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const = 0; + + /// \overload for 2D points + virtual void nn_multiple_search( + const mrpt::math::TPoint2Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const = 0; + + /** Radius search for closest 3D points to a given one. + * + * \param[in] query The query input point. + * \param[in] search_radius_sqr The search radius, **squared**. + * \param[out] results The found closest points. + * \param[out] out_dists_sqr The square Euclidean distances between the + * query and the returned point. + */ + virtual void nn_radius_search( + const mrpt::math::TPoint3Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const = 0; + + /// \overload for 2D points + virtual void nn_radius_search( + const mrpt::math::TPoint2Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const = 0; + + /** @} */ +}; + +} // namespace mrpt::maps diff --git a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp index 7136f79706..99aa0febf8 100644 --- a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp +++ b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp @@ -639,8 +639,7 @@ void COccupancyGridMap2D::determineMatching2D( for (int cy = cy_min; cy <= cy_max; cy++) { // Is an occupied cell? - if (m_map[cx + cy * m_size_x] < - thresholdCellValue) // getCell(cx,cy)<0.49) + if (m_map[cx + cy * m_size_x] < thresholdCellValue) { const float residual_x = idx2x(cx) - x_local; const float residual_y = idx2y(cy) - y_local; @@ -768,3 +767,65 @@ float COccupancyGridMap2D::compute3DMatchingRatio( { return 0; } + +bool COccupancyGridMap2D::nn_single_search( + const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, + float& out_dist_sqr) const +{ + // delegate to the 2D version: + mrpt::math::TPoint2Df r; + bool res = nn_single_search({query.x, query.y}, r, out_dist_sqr); + result = {r.x, r.y, .0f}; + return res; +} +void COccupancyGridMap2D::nn_multiple_search( + const mrpt::math::TPoint3Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const +{ + // delegate to the 2D version: + std::vector r; + nn_multiple_search({query.x, query.y}, N, r, out_dists_sqr); + results.resize(r.size()); + for (size_t i = 0; i < r.size(); i++) + results[i] = {r[i].x, r[i].y, .0f}; +} +void COccupancyGridMap2D::nn_radius_search( + const mrpt::math::TPoint3Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const +{ + // delegate to the 2D version: + std::vector r; + nn_radius_search({query.x, query.y}, search_radius_sqr, r, out_dists_sqr); + results.resize(r.size()); + for (size_t i = 0; i < r.size(); i++) + results[i] = {r[i].x, r[i].y, .0f}; +} + +bool COccupancyGridMap2D::nn_single_search( + const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, + float& out_dist_sqr) const +{ + std::vector r; + std::vector dist_sqr; + nn_multiple_search(query, 1, r, dist_sqr); + if (r.empty()) return false; // none found + result = r[0]; + out_dist_sqr = dist_sqr[0]; + return true; +} + +void COccupancyGridMap2D::nn_multiple_search( + const mrpt::math::TPoint2Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const +{ +} + +void COccupancyGridMap2D::nn_radius_search( + const mrpt::math::TPoint2Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const +{ +} diff --git a/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp b/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp index 6f425dcf97..6159506c54 100644 --- a/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp +++ b/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp @@ -37,6 +37,20 @@ TEST(COccupancyGridMap2DTests, insert2DScan) } } +TEST(COccupancyGridMap2DTests, NearestNeighborsCapable) +{ + // low freeness=occupied + const float occupied = 0.2f; + const std::vector occupiedPoints = { + {1.0f, 2.0f}, {1.1f, 2.0f}, {1.2f, 2.0f}, {2.0f, 3.0f}}; + + COccupancyGridMap2D grid(-10.0f, 10.0f, -10.0f, 10.0f, 0.10f); + for (const auto& pt : occupiedPoints) + grid.setCell(grid.x2idx(pt.x), grid.x2idx(pt.y), occupied); + + MRPT_TODO("continue"); +} + // We need OPENCV to read the image. #if MRPT_HAS_OPENCV && MRPT_HAS_FYAML @@ -56,4 +70,4 @@ TEST(COccupancyGridMap2DTests, loadFromROSMapServerYAML) ASSERT_EQUAL_(grid.getPos(2.0, 2.0), 0.5f); } -#endif \ No newline at end of file +#endif diff --git a/libs/maps/src/maps/CPointsMap.cpp b/libs/maps/src/maps/CPointsMap.cpp index aeeb1ce762..8e662503bf 100644 --- a/libs/maps/src/maps/CPointsMap.cpp +++ b/libs/maps/src/maps/CPointsMap.cpp @@ -2097,3 +2097,97 @@ void CPointsMap::loadFromVelodyneScan( ); } } + +// =========== API of the NearestNeighborsCapable virtual interface ====== +// See docs in base class +bool CPointsMap::nn_single_search( + const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, + float& out_dist_sqr) const +{ + try + { + kdTreeClosestPoint3D( + query.x, query.y, query.z, result.x, result.y, result.z, + out_dist_sqr); + return true; + } + catch (const std::exception&) + { + // kdTree*() methods throw on empty map + return false; + } +} +bool CPointsMap::nn_single_search( + const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, + float& out_dist_sqr) const +{ + try + { + kdTreeClosestPoint2D( + query.x, query.y, result.x, result.y, out_dist_sqr); + return true; + } + catch (const std::exception&) + { + // kdTree*() methods throw on empty map + return false; + } +} +void CPointsMap::nn_multiple_search( + const mrpt::math::TPoint3Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const +{ + std::vector idxs; + kdTreeNClosestPoint3DIdx(query.x, query.y, query.z, N, idxs, out_dists_sqr); + results.resize(idxs.size()); + for (size_t i = 0; i < idxs.size(); i++) + getPointFast(idxs[i], results[i].x, results[i].y, results[i].z); +} +void CPointsMap::nn_multiple_search( + const mrpt::math::TPoint2Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const +{ + std::vector idxs; + kdTreeNClosestPoint2DIdx(query.x, query.y, N, idxs, out_dists_sqr); + results.resize(idxs.size()); + float dummyZ = 0; + for (size_t i = 0; i < idxs.size(); i++) + getPointFast(idxs[i], results[i].x, results[i].y, dummyZ); +} +void CPointsMap::nn_radius_search( + const mrpt::math::TPoint3Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const +{ + std::vector> indices_dist; + kdTreeRadiusSearch3D( + query.x, query.y, query.z, search_radius_sqr, indices_dist); + const size_t nResults = indices_dist.size(); + results.resize(nResults); + out_dists_sqr.resize(nResults); + for (size_t i = 0; i < nResults; i++) + { + getPointFast( + indices_dist[i].first, results[i].x, results[i].y, results[i].z); + out_dists_sqr[i] = indices_dist[i].second; + } +} +void CPointsMap::nn_radius_search( + const mrpt::math::TPoint2Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const +{ + std::vector> indices_dist; + kdTreeRadiusSearch2D(query.x, query.y, search_radius_sqr, indices_dist); + const size_t nResults = indices_dist.size(); + results.resize(nResults); + out_dists_sqr.resize(nResults); + float dummyZ = 0; + for (size_t i = 0; i < nResults; i++) + { + getPointFast(indices_dist[i].first, results[i].x, results[i].y, dummyZ); + out_dists_sqr[i] = indices_dist[i].second; + } +} From bc29ce552cd2ba5f5217729a073b3b7ac51a4e87 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 5 Nov 2023 00:48:38 +0100 Subject: [PATCH 06/36] More "nodiscard" attributes --- doc/source/doxygen-docs/changelog.md | 2 ++ libs/core/include/mrpt/core/bits_math.h | 26 ++++++++++++++----------- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index b53b0675b0..2957162297 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -9,6 +9,8 @@ - All classes derived from mrpt::maps::CVoxelMapOccupancyBase - mrpt::maps::COccupancyGridMap2D - mrpt::maps::COccupancyGridMap2D + - \ref mrpt_core_grp + - Add the `[[nodiscard]]` attribute to all functions returning a value in `` # Version 2.11.2: Released Oct 25th, 2023 - Changes in libraries: diff --git a/libs/core/include/mrpt/core/bits_math.h b/libs/core/include/mrpt/core/bits_math.h index 8268b9fab9..3a7ab85a34 100644 --- a/libs/core/include/mrpt/core/bits_math.h +++ b/libs/core/include/mrpt/core/bits_math.h @@ -94,21 +94,21 @@ using namespace mrpt::literals; // for backwards compatib. /** Returns the sign of X as "1" or "-1" */ template -inline int sign(T x) +[[nodiscard]] inline int sign(T x) { return x < 0 ? -1 : 1; } /** Returns the sign of X as "0", "1" or "-1" */ template -inline int signWithZero(T x) +[[nodiscard]] inline int signWithZero(T x) { return (x == 0 || x == -0) ? 0 : sign(x); } /** Returns the smallest positive number among a and b */ template -T lowestPositive(const T a, const T b) +[[nodiscard]] T lowestPositive(const T a, const T b) { if (a > 0 && a <= b) return a; // a positive and smaller than b else if (b > 0) @@ -118,19 +118,19 @@ T lowestPositive(const T a, const T b) } template -inline const T min3(const T& A, const T& B, const T& C) +[[nodiscard]] inline const T min3(const T& A, const T& B, const T& C) { return std::min(A, std::min(B, C)); } template -inline const T max3(const T& A, const T& B, const T& C) +[[nodiscard]] inline const T max3(const T& A, const T& B, const T& C) { return std::max(A, std::max(B, C)); } /** Rounds toward zero */ template -inline int fix(T x) +[[nodiscard]] inline int fix(T x) { return x > 0 ? static_cast(floor(static_cast(x))) : static_cast(ceil(static_cast(x))); @@ -161,7 +161,8 @@ inline void saturate(T& var, const T sat_min, const T sat_max) /** Like saturate() but it returns the value instead of modifying the variable */ template -inline T saturate_val(const T& value, const T sat_min, const T sat_max) +[[nodiscard]] inline T saturate_val( + const T& value, const T sat_min, const T sat_max) { T var = value; if (var > sat_max) var = sat_max; @@ -171,7 +172,7 @@ inline T saturate_val(const T& value, const T sat_min, const T sat_max) /** Round up to the nearest power of two of a given number */ template -T round2up(T val) +[[nodiscard]] T round2up(T val) { T n = 1; while (n < val) @@ -183,14 +184,17 @@ T round2up(T val) } /** shortcut for static_cast(double) */ -inline float d2f(const double d) { return static_cast(d); } +[[nodiscard]] inline float d2f(const double d) { return static_cast(d); } /** converts a float [0,1] into an uint8_t [0,255] (without checking for out of * bounds) \sa u8tof */ -inline uint8_t f2u8(const float f) { return static_cast(f * 255); } +[[nodiscard]] inline uint8_t f2u8(const float f) +{ + return static_cast(f * 255); +} /** converts a uint8_t [0,255] into a float [0,1] \sa f2u8 */ -inline float u8tof(const uint8_t v) { return v / 255.0f; } +[[nodiscard]] inline float u8tof(const uint8_t v) { return v / 255.0f; } /** @} */ From b6f687144b1ce9e319cf414c196f6a0f54a52c0d Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 5 Nov 2023 00:49:15 +0100 Subject: [PATCH 07/36] Fix clang-tidy apparent false warnings on uninitialized values --- libs/math/include/mrpt/math/TPoint2D.h | 3 +++ libs/math/include/mrpt/math/TPoint3D.h | 3 +++ 2 files changed, 6 insertions(+) diff --git a/libs/math/include/mrpt/math/TPoint2D.h b/libs/math/include/mrpt/math/TPoint2D.h index 1f86af47f4..6a4bdcc831 100644 --- a/libs/math/include/mrpt/math/TPoint2D.h +++ b/libs/math/include/mrpt/math/TPoint2D.h @@ -25,6 +25,9 @@ namespace mrpt::math template struct TPoint2D_data { + TPoint2D_data() = default; + TPoint2D_data(T X, T Y) : x(X), y(Y) {} + /** X,Y coordinates */ T x, y; }; diff --git a/libs/math/include/mrpt/math/TPoint3D.h b/libs/math/include/mrpt/math/TPoint3D.h index 8f5a947982..ba730b091c 100644 --- a/libs/math/include/mrpt/math/TPoint3D.h +++ b/libs/math/include/mrpt/math/TPoint3D.h @@ -30,6 +30,9 @@ namespace mrpt::math template struct TPoint3D_data { + TPoint3D_data() = default; + TPoint3D_data(T X, T Y, T Z) : x(X), y(Y), z(Z) {} + /** X,Y,Z coordinates */ T x, y, z; }; From 50297c33f58e8d938d69426d5d6a74c9cdba5799 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 5 Nov 2023 00:49:35 +0100 Subject: [PATCH 08/36] Impl NN interface in grid2d --- .../src/maps/COccupancyGridMap2D_common.cpp | 117 ++++++++++++++++++ .../src/maps/COccupancyGridMap2D_unittest.cpp | 80 +++++++++++- 2 files changed, 194 insertions(+), 3 deletions(-) diff --git a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp index 99aa0febf8..0f59d9b86e 100644 --- a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp +++ b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp @@ -821,6 +821,67 @@ void COccupancyGridMap2D::nn_multiple_search( std::vector& results, std::vector& out_dists_sqr) const { + results.clear(); + results.reserve(N); + out_dists_sqr.clear(); + out_dists_sqr.reserve(N); + + int cx_query = x2idx(query.x), cy_query = y2idx(query.y); + + mrpt::saturate(cx_query, 0, getSizeX() - 1); + mrpt::saturate(cy_query, 0, getSizeY() - 1); + + const cellType thresholdCellValue = p2l(0.5f); + const float resolutionSqr = mrpt::square(m_resolution); + + for (int searchRadiusInCells = 0; results.size() < N && + searchRadiusInCells < std::max(getSizeX(), getSizeY()); + searchRadiusInCells++) + { + int cx0 = cx_query - searchRadiusInCells; + int cx1 = cx_query + searchRadiusInCells; + int cy0 = cy_query - searchRadiusInCells; + int cy1 = cy_query + searchRadiusInCells; + + mrpt::saturate(cx0, 0, getSizeX() - 1); + mrpt::saturate(cy0, 0, getSizeY() - 1); + mrpt::saturate(cx1, 0, getSizeX() - 1); + mrpt::saturate(cy1, 0, getSizeY() - 1); + + std::map> dists2cells; + + auto lambdaAddCell = [&dists2cells, cx_query, cy_query]( + int cx, int cy) { + int distSqr = + mrpt::square(cx - cx_query) + mrpt::square(cy - cy_query); + dists2cells[distSqr] = {cx, cy}; + }; + + for (int cx = cx0; cx <= cx1; cx++) + { + if (int cy = cy0; m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + if (int cy = cy1; + cy1 != cy0 && m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + } + for (int cy = cy0 + 1; cy < cy1; cy++) + { + if (int cx = cx0; m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + if (int cx = cx1; m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + } + + // Add the top best "N" neighbors: + for (auto it = dists2cells.begin(); + it != dists2cells.end() && results.size() < N; ++it) + { + out_dists_sqr.push_back(it->first * resolutionSqr); + results.push_back( + {idx2x(it->second.first), idx2y(it->second.second)}); + } + } } void COccupancyGridMap2D::nn_radius_search( @@ -828,4 +889,60 @@ void COccupancyGridMap2D::nn_radius_search( std::vector& results, std::vector& out_dists_sqr) const { + results.clear(); + out_dists_sqr.clear(); + + if (search_radius_sqr == 0) return; + + int cx_query = x2idx(query.x), cy_query = y2idx(query.y); + + mrpt::saturate(cx_query, 0, getSizeX() - 1); + mrpt::saturate(cy_query, 0, getSizeY() - 1); + + const cellType thresholdCellValue = p2l(0.5f); + const float resolutionSqr = mrpt::square(m_resolution); + const int maxSearchRadiusInCells = static_cast( + std::ceil(std::sqrt(search_radius_sqr) / m_resolution)); + const int maxSearchRadiusSqrInCells = mrpt::square(maxSearchRadiusInCells); + + for (int searchRadiusInCells = 0; + searchRadiusInCells <= maxSearchRadiusInCells; searchRadiusInCells++) + { + int cx0 = cx_query - searchRadiusInCells; + int cx1 = cx_query + searchRadiusInCells; + int cy0 = cy_query - searchRadiusInCells; + int cy1 = cy_query + searchRadiusInCells; + + mrpt::saturate(cx0, 0, getSizeX() - 1); + mrpt::saturate(cy0, 0, getSizeY() - 1); + mrpt::saturate(cx1, 0, getSizeX() - 1); + mrpt::saturate(cy1, 0, getSizeY() - 1); + + auto lambdaAddCell = [maxSearchRadiusSqrInCells, cx_query, cy_query, + &out_dists_sqr, &results, resolutionSqr, + this](int cx, int cy) { + int distSqr = + mrpt::square(cx - cx_query) + mrpt::square(cy - cy_query); + if (distSqr > maxSearchRadiusSqrInCells) return; + + out_dists_sqr.push_back(distSqr * resolutionSqr); + results.push_back({idx2x(cx), idx2y(cy)}); + }; + + for (int cx = cx0; cx <= cx1; cx++) + { + if (int cy = cy0; m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + if (int cy = cy1; + cy1 != cy0 && m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + } + for (int cy = cy0 + 1; cy < cy1; cy++) + { + if (int cx = cx0; m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + if (int cx = cx1; m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + } + } } diff --git a/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp b/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp index 6159506c54..a57b69e6a0 100644 --- a/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp +++ b/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp @@ -40,15 +40,89 @@ TEST(COccupancyGridMap2DTests, insert2DScan) TEST(COccupancyGridMap2DTests, NearestNeighborsCapable) { // low freeness=occupied - const float occupied = 0.2f; + const float occupied = 0.2f, resolution = 0.10f; const std::vector occupiedPoints = { {1.0f, 2.0f}, {1.1f, 2.0f}, {1.2f, 2.0f}, {2.0f, 3.0f}}; - COccupancyGridMap2D grid(-10.0f, 10.0f, -10.0f, 10.0f, 0.10f); + COccupancyGridMap2D grid(-10.0f, 10.0f, -10.0f, 10.0f, resolution); for (const auto& pt : occupiedPoints) grid.setCell(grid.x2idx(pt.x), grid.x2idx(pt.y), occupied); - MRPT_TODO("continue"); + mrpt::maps::NearestNeighborsCapable& nn = + dynamic_cast(grid); + + { + mrpt::math::TPoint2Df result; + float out_dist_sqr = 0; + bool found = nn.nn_single_search({0.90f, 1.95f}, result, out_dist_sqr); + EXPECT_TRUE(found); + EXPECT_NEAR(result.x, 1.0f, resolution); + EXPECT_NEAR(result.y, 2.0f, resolution); + EXPECT_NEAR(std::sqrt(out_dist_sqr), 0.15f, resolution); + } + { + mrpt::math::TPoint2Df result; + float out_dist_sqr = 0; + bool found = nn.nn_single_search({-4.0f, 2.1f}, result, out_dist_sqr); + EXPECT_TRUE(found); + EXPECT_NEAR(result.x, 1.0f, resolution); + EXPECT_NEAR(result.y, 2.0f, resolution); + EXPECT_NEAR(std::sqrt(out_dist_sqr), 5.0f, resolution); + } + + { + std::vector results; + std::vector out_dists_sqr; + nn.nn_multiple_search({-2.0f, 5.0f}, 2, results, out_dists_sqr); + + EXPECT_EQ(results.size(), 2UL); + EXPECT_EQ(out_dists_sqr.size(), results.size()); + + EXPECT_NEAR(results.at(0).x, 1.0f, resolution); + EXPECT_NEAR(results.at(0).y, 2.0f, resolution); + EXPECT_NEAR( + std::sqrt(out_dists_sqr.at(0)), + std::hypot(-2.0f - 1.0f, 5.0f - 2.0f), resolution); + + EXPECT_NEAR(results.at(1).x, 1.1f, resolution); + EXPECT_NEAR(results.at(1).y, 2.0f, resolution); + EXPECT_NEAR( + std::sqrt(out_dists_sqr.at(1)), + std::hypot(-2.0f - 1.1f, 5.0f - 2.0f), resolution); + } + { + std::vector results; + std::vector out_dists_sqr; + nn.nn_radius_search( + {-2.0f, 5.0f}, mrpt::square(10.0f), results, out_dists_sqr); + + EXPECT_EQ(results.size(), occupiedPoints.size()); + EXPECT_EQ(out_dists_sqr.size(), results.size()); + + EXPECT_NEAR(results.at(0).x, 1.0f, resolution); + EXPECT_NEAR(results.at(0).y, 2.0f, resolution); + EXPECT_NEAR( + std::sqrt(out_dists_sqr.at(0)), + std::hypot(-2.0f - 1.0f, 5.0f - 2.0f), resolution); + } + { + std::vector results; + std::vector out_dists_sqr; + nn.nn_radius_search( + {0.9f, 1.9f}, mrpt::square(1.0f), results, out_dists_sqr); + + EXPECT_EQ(results.size(), 3UL); + EXPECT_EQ(out_dists_sqr.size(), results.size()); + } + { + std::vector results; + std::vector out_dists_sqr; + nn.nn_radius_search( + {0.5f, 1.5f}, mrpt::square(0.5f), results, out_dists_sqr); + + EXPECT_EQ(results.size(), 0UL); + EXPECT_EQ(out_dists_sqr.size(), results.size()); + } } // We need OPENCV to read the image. From e0a2e8a6642fdd4f5d3aa413e520c363121cc974 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 5 Nov 2023 01:21:20 +0100 Subject: [PATCH 09/36] NN API impl for 3D grid too --- .../include/mrpt/maps/COccupancyGridMap3D.h | 31 ++- .../src/maps/COccupancyGridMap2D_common.cpp | 9 +- libs/maps/src/maps/COccupancyGridMap3D.cpp | 223 ++++++++++++++++++ .../src/maps/COccupancyGridMap3D_unittest.cpp | 94 ++++++++ 4 files changed, 353 insertions(+), 4 deletions(-) diff --git a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h index 97ea8d061e..0710d9d540 100644 --- a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h +++ b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -40,7 +41,8 @@ namespace mrpt::maps **/ class COccupancyGridMap3D : public CMetricMap, - public CLogOddsGridMap3D + public CLogOddsGridMap3D, + public mrpt::maps::NearestNeighborsCapable { DEFINE_SERIALIZABLE(COccupancyGridMap3D, mrpt::maps) public: @@ -387,6 +389,33 @@ class COccupancyGridMap3D m_grid.getResolutionXY(), m_grid.getResolutionZ()); } + /** @name API of the NearestNeighborsCapable virtual interface + @{ */ + // See docs in base class + [[nodiscard]] bool nn_single_search( + const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, + float& out_dist_sqr) const override; + [[nodiscard]] bool nn_single_search( + const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, + float& out_dist_sqr) const override; + void nn_multiple_search( + const mrpt::math::TPoint3Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const override; + void nn_multiple_search( + const mrpt::math::TPoint2Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const override; + void nn_radius_search( + const mrpt::math::TPoint3Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const override; + void nn_radius_search( + const mrpt::math::TPoint2Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const override; + /** @} */ + private: // See docs in base class double internal_computeObservationLikelihood( diff --git a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp index 0f59d9b86e..cf61c9fb75 100644 --- a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp +++ b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp @@ -840,8 +840,11 @@ void COccupancyGridMap2D::nn_multiple_search( { int cx0 = cx_query - searchRadiusInCells; int cx1 = cx_query + searchRadiusInCells; + if (cx1 < 0 || cx0 >= static_cast(getSizeX())) continue; + int cy0 = cy_query - searchRadiusInCells; int cy1 = cy_query + searchRadiusInCells; + if (cy1 < 0 || cy0 >= static_cast(getSizeY())) continue; mrpt::saturate(cx0, 0, getSizeX() - 1); mrpt::saturate(cy0, 0, getSizeY() - 1); @@ -896,9 +899,6 @@ void COccupancyGridMap2D::nn_radius_search( int cx_query = x2idx(query.x), cy_query = y2idx(query.y); - mrpt::saturate(cx_query, 0, getSizeX() - 1); - mrpt::saturate(cy_query, 0, getSizeY() - 1); - const cellType thresholdCellValue = p2l(0.5f); const float resolutionSqr = mrpt::square(m_resolution); const int maxSearchRadiusInCells = static_cast( @@ -910,8 +910,11 @@ void COccupancyGridMap2D::nn_radius_search( { int cx0 = cx_query - searchRadiusInCells; int cx1 = cx_query + searchRadiusInCells; + if (cx1 < 0 || cx0 >= static_cast(getSizeX())) continue; + int cy0 = cy_query - searchRadiusInCells; int cy1 = cy_query + searchRadiusInCells; + if (cy1 < 0 || cy0 >= static_cast(getSizeY())) continue; mrpt::saturate(cx0, 0, getSizeX() - 1); mrpt::saturate(cy0, 0, getSizeY() - 1); diff --git a/libs/maps/src/maps/COccupancyGridMap3D.cpp b/libs/maps/src/maps/COccupancyGridMap3D.cpp index c73c49ef88..1a578cd222 100644 --- a/libs/maps/src/maps/COccupancyGridMap3D.cpp +++ b/libs/maps/src/maps/COccupancyGridMap3D.cpp @@ -490,3 +490,226 @@ void COccupancyGridMap3D::saveMetricMapRepresentationToFile( const auto fil = filNamePrefix + ".txt"; // todo } + +bool COccupancyGridMap3D::nn_single_search( + const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, + float& out_dist_sqr) const +{ + THROW_EXCEPTION("Cannot run a 2D search on a 3D gridmap"); +} +void COccupancyGridMap3D::nn_multiple_search( + const mrpt::math::TPoint2Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const +{ + THROW_EXCEPTION("Cannot run a 2D search on a 3D gridmap"); +} +void COccupancyGridMap3D::nn_radius_search( + const mrpt::math::TPoint2Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const +{ + THROW_EXCEPTION("Cannot run a 2D search on a 3D gridmap"); +} + +bool COccupancyGridMap3D::nn_single_search( + const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, + float& out_dist_sqr) const +{ + std::vector r; + std::vector dist_sqr; + nn_multiple_search(query, 1, r, dist_sqr); + if (r.empty()) return false; // none found + result = r[0]; + out_dist_sqr = dist_sqr[0]; + return true; +} + +void COccupancyGridMap3D::nn_multiple_search( + const mrpt::math::TPoint3Df& query, const size_t N, + std::vector& results, + std::vector& out_dists_sqr) const +{ + results.clear(); + results.reserve(N); + out_dists_sqr.clear(); + out_dists_sqr.reserve(N); + + int cx_query = m_grid.x2idx(query.x), cy_query = m_grid.y2idx(query.y), + cz_query = m_grid.z2idx(query.z); + + const int sizeX = static_cast(m_grid.getSizeX()); + const int sizeY = static_cast(m_grid.getSizeY()); + const int sizeZ = static_cast(m_grid.getSizeZ()); + + mrpt::saturate(cx_query, 0, sizeX - 1); + mrpt::saturate(cy_query, 0, sizeY - 1); + mrpt::saturate(cz_query, 0, sizeZ - 1); + + const voxelType thresholdCellValue = p2l(0.5f); + const float resolutionSqr = mrpt::square(m_grid.getResolutionXY()); + + for (int searchRadiusInCells = 0; + results.size() < N && searchRadiusInCells < std::max(sizeX, sizeY); + searchRadiusInCells++) + { + int cx0 = cx_query - searchRadiusInCells; + int cx1 = cx_query + searchRadiusInCells; + if (cx1 < 0 || cx0 >= sizeX) continue; + + int cy0 = cy_query - searchRadiusInCells; + int cy1 = cy_query + searchRadiusInCells; + if (cy1 < 0 || cy0 >= sizeY) continue; + + int cz0 = cz_query - searchRadiusInCells; + int cz1 = cz_query + searchRadiusInCells; + if (cz1 < 0 || cz0 >= sizeZ) continue; + + mrpt::saturate(cx0, 0, sizeX - 1); + mrpt::saturate(cy0, 0, sizeY - 1); + mrpt::saturate(cz0, 0, sizeZ - 1); + mrpt::saturate(cx1, 0, sizeX - 1); + mrpt::saturate(cy1, 0, sizeY - 1); + mrpt::saturate(cz1, 0, sizeZ - 1); + + std::map> dists2cells; + + auto lambdaAddCell = [&dists2cells, cx_query, cy_query, cz_query]( + int cx, int cy, int cz) { + int distSqr = mrpt::square(cx - cx_query) + + mrpt::square(cy - cy_query) + mrpt::square(cz - cz_query); + dists2cells[distSqr] = {cx, cy, cz}; + }; + + for (int cx = cx0; cx <= cx1; cx++) + { + for (int cz = cz0; cz <= cz1; cz++) + { + int cy = cy0; + if (auto* c = m_grid.cellByIndex(cx, cy, cz); + c && *c < thresholdCellValue) + lambdaAddCell(cx, cy, cz); + cy = cy1; + if (auto* c = m_grid.cellByIndex(cx, cy, cz); + c && *c < thresholdCellValue) + lambdaAddCell(cx, cy, cz); + } + } + for (int cy = cy0 + 1; cy < cy1; cy++) + { + for (int cz = cz0; cz <= cz1; cz++) + { + int cx = cx0; + if (auto* c = m_grid.cellByIndex(cx, cy, cz); + c && *c < thresholdCellValue) + lambdaAddCell(cx, cy, cz); + cx = cx1; + if (auto* c = m_grid.cellByIndex(cx, cy, cz); + c && *c < thresholdCellValue) + lambdaAddCell(cx, cy, cz); + } + } + + // Add the top best "N" neighbors: + for (auto it = dists2cells.begin(); + it != dists2cells.end() && results.size() < N; ++it) + { + out_dists_sqr.push_back(it->first * resolutionSqr); + results.push_back(mrpt::math::TPoint3Df( + m_grid.idx2x(it->second[0]), m_grid.idx2y(it->second[1]), + m_grid.idx2z(it->second[2]))); + } + } +} + +void COccupancyGridMap3D::nn_radius_search( + const mrpt::math::TPoint3Df& query, const float search_radius_sqr, + std::vector& results, + std::vector& out_dists_sqr) const +{ + results.clear(); + out_dists_sqr.clear(); + + if (search_radius_sqr == 0) return; + + int cx_query = m_grid.x2idx(query.x), cy_query = m_grid.y2idx(query.y), + cz_query = m_grid.z2idx(query.z); + + const int sizeX = static_cast(m_grid.getSizeX()); + const int sizeY = static_cast(m_grid.getSizeY()); + const int sizeZ = static_cast(m_grid.getSizeZ()); + + mrpt::saturate(cx_query, 0, sizeX - 1); + mrpt::saturate(cy_query, 0, sizeY - 1); + mrpt::saturate(cz_query, 0, sizeZ - 1); + + const voxelType thresholdCellValue = p2l(0.5f); + const float resolutionSqr = mrpt::square(m_grid.getResolutionXY()); + const int maxSearchRadiusInCells = static_cast( + std::ceil(std::sqrt(search_radius_sqr) / m_grid.getResolutionXY())); + const int maxSearchRadiusSqrInCells = mrpt::square(maxSearchRadiusInCells); + + for (int searchRadiusInCells = 0; + searchRadiusInCells <= maxSearchRadiusInCells; searchRadiusInCells++) + { + int cx0 = cx_query - searchRadiusInCells; + int cx1 = cx_query + searchRadiusInCells; + if (cx1 < 0 || cx0 >= sizeX) continue; + + int cy0 = cy_query - searchRadiusInCells; + int cy1 = cy_query + searchRadiusInCells; + if (cy1 < 0 || cy0 >= sizeY) continue; + + int cz0 = cz_query - searchRadiusInCells; + int cz1 = cz_query + searchRadiusInCells; + if (cz1 < 0 || cz0 >= sizeZ) continue; + + mrpt::saturate(cx0, 0, sizeX - 1); + mrpt::saturate(cy0, 0, sizeY - 1); + mrpt::saturate(cz0, 0, sizeZ - 1); + mrpt::saturate(cx1, 0, sizeX - 1); + mrpt::saturate(cy1, 0, sizeY - 1); + mrpt::saturate(cz1, 0, sizeZ - 1); + + auto lambdaAddCell = [maxSearchRadiusSqrInCells, cx_query, cy_query, + cz_query, &out_dists_sqr, &results, resolutionSqr, + this](int cx, int cy, int cz) { + int distSqr = mrpt::square(cx - cx_query) + + mrpt::square(cy - cy_query) + mrpt::square(cz - cz_query); + if (distSqr > maxSearchRadiusSqrInCells) return; + + out_dists_sqr.push_back(distSqr * resolutionSqr); + results.emplace_back( + m_grid.idx2x(cx), m_grid.idx2y(cy), m_grid.idx2z(cz)); + }; + + for (int cx = cx0; cx <= cx1; cx++) + { + for (int cz = cz0; cz <= cz1; cz++) + { + int cy = cy0; + if (auto* c = m_grid.cellByIndex(cx, cy, cz); + c && *c < thresholdCellValue) + lambdaAddCell(cx, cy, cz); + cy = cy1; + if (auto* c = m_grid.cellByIndex(cx, cy, cz); + c && *c < thresholdCellValue) + lambdaAddCell(cx, cy, cz); + } + } + for (int cy = cy0 + 1; cy < cy1; cy++) + { + for (int cz = cz0; cz <= cz1; cz++) + { + int cx = cx0; + if (auto* c = m_grid.cellByIndex(cx, cy, cz); + c && *c < thresholdCellValue) + lambdaAddCell(cx, cy, cz); + cx = cx1; + if (auto* c = m_grid.cellByIndex(cx, cy, cz); + c && *c < thresholdCellValue) + lambdaAddCell(cx, cy, cz); + } + } + } +} diff --git a/libs/maps/src/maps/COccupancyGridMap3D_unittest.cpp b/libs/maps/src/maps/COccupancyGridMap3D_unittest.cpp index 1b266535cd..08bedac437 100644 --- a/libs/maps/src/maps/COccupancyGridMap3D_unittest.cpp +++ b/libs/maps/src/maps/COccupancyGridMap3D_unittest.cpp @@ -33,6 +33,100 @@ TEST(COccupancyGridMap3DTests, insert2DScan) } } +TEST(COccupancyGridMap3DTests, NearestNeighborsCapable) +{ + // low freeness=occupied + const float occupied = 0.2f, resolution = 0.10f; + const std::vector occupiedPoints = { + {1.0f, 2.0f, 1.0f}, + {1.1f, 2.0f, 1.0f}, + {1.2f, 2.0f, 1.0f}, + {2.0f, 3.0f, 2.0f}}; + + mrpt::maps::COccupancyGridMap3D grid( + {-10.0f, -10.0f, -5.0f}, {10.0f, 10.0f, 50.0f}, resolution); + for (const auto& pt : occupiedPoints) + grid.setFreenessByPos(pt.x, pt.y, pt.z, occupied); + + mrpt::maps::NearestNeighborsCapable& nn = + dynamic_cast(grid); + + { + mrpt::math::TPoint3Df result; + float out_dist_sqr = 0; + bool found = + nn.nn_single_search({0.90f, 1.95f, 1.0f}, result, out_dist_sqr); + EXPECT_TRUE(found); + EXPECT_NEAR(result.x, 1.0f, resolution); + EXPECT_NEAR(result.y, 2.0f, resolution); + EXPECT_NEAR(std::sqrt(out_dist_sqr), 0.15f, resolution); + } + { + mrpt::math::TPoint3Df result; + float out_dist_sqr = 0; + bool found = + nn.nn_single_search({-4.0f, 2.1f, 1.0f}, result, out_dist_sqr); + EXPECT_TRUE(found); + EXPECT_NEAR(result.x, 1.0f, resolution); + EXPECT_NEAR(result.y, 2.0f, resolution); + EXPECT_NEAR(std::sqrt(out_dist_sqr), 5.0f, resolution); + } + + { + std::vector results; + std::vector out_dists_sqr; + nn.nn_multiple_search({-2.0f, 5.0f, 1.0f}, 2, results, out_dists_sqr); + + EXPECT_EQ(results.size(), 2UL); + EXPECT_EQ(out_dists_sqr.size(), results.size()); + + EXPECT_NEAR(results.at(0).x, 1.0f, resolution); + EXPECT_NEAR(results.at(0).y, 2.0f, resolution); + EXPECT_NEAR( + std::sqrt(out_dists_sqr.at(0)), + std::hypot(-2.0f - 1.0f, 5.0f - 2.0f), resolution); + + EXPECT_NEAR(results.at(1).x, 1.1f, resolution); + EXPECT_NEAR(results.at(1).y, 2.0f, resolution); + EXPECT_NEAR( + std::sqrt(out_dists_sqr.at(1)), + std::hypot(-2.0f - 1.1f, 5.0f - 2.0f), resolution); + } + { + std::vector results; + std::vector out_dists_sqr; + nn.nn_radius_search( + {-2.0f, 5.0f, 1.0f}, mrpt::square(10.0f), results, out_dists_sqr); + + EXPECT_EQ(results.size(), occupiedPoints.size()); + EXPECT_EQ(out_dists_sqr.size(), results.size()); + + EXPECT_NEAR(results.at(0).x, 1.0f, resolution); + EXPECT_NEAR(results.at(0).y, 2.0f, resolution); + EXPECT_NEAR( + std::sqrt(out_dists_sqr.at(0)), + std::hypot(-2.0f - 1.0f, 5.0f - 2.0f), resolution); + } + { + std::vector results; + std::vector out_dists_sqr; + nn.nn_radius_search( + {0.9f, 1.9f, 1.0f}, mrpt::square(1.0f), results, out_dists_sqr); + + EXPECT_EQ(results.size(), 3UL); + EXPECT_EQ(out_dists_sqr.size(), results.size()); + } + { + std::vector results; + std::vector out_dists_sqr; + nn.nn_radius_search( + {0.5f, 1.5f, 1.0f}, mrpt::square(0.5f), results, out_dists_sqr); + + EXPECT_EQ(results.size(), 0UL); + EXPECT_EQ(out_dists_sqr.size(), results.size()); + } +} + // We need OPENCV to read the image internal to CObservation3DRangeScan, // so skip this test if built without opencv. #if MRPT_HAS_OPENCV From d4b407c7d34f72468882390e44d2e6a476fb40ce Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 5 Nov 2023 01:46:26 +0100 Subject: [PATCH 10/36] Add new virtual method CMetricMap::boundingBox() --- doc/source/doxygen-docs/changelog.md | 1 + libs/maps/include/mrpt/maps/COccupancyGridMap2D.h | 7 +++++++ libs/maps/include/mrpt/maps/COccupancyGridMap3D.h | 9 +++++++++ libs/maps/include/mrpt/maps/CPointsMap.h | 2 +- .../include/mrpt/maps/CVoxelMapOccupancyBase.h | 15 ++++++++------- libs/obs/include/mrpt/maps/CMetricMap.h | 10 ++++++++++ libs/opengl/src/CPointCloud.cpp | 1 - 7 files changed, 36 insertions(+), 9 deletions(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 2957162297..5c6ecd830a 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -9,6 +9,7 @@ - All classes derived from mrpt::maps::CVoxelMapOccupancyBase - mrpt::maps::COccupancyGridMap2D - mrpt::maps::COccupancyGridMap2D + - New virtual method mrpt::maps::CMetricMap::boundingBox() - \ref mrpt_core_grp - Add the `[[nodiscard]]` attribute to all functions returning a value in `` diff --git a/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h b/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h index 7713971815..f91886f849 100644 --- a/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h +++ b/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h @@ -328,6 +328,13 @@ class COccupancyGridMap2D return static_cast((y - ymin) / m_resolution); } + mrpt::math::TBoundingBoxf boundingBox() const override + { + return { + {m_xMin, m_yMin, insertionOptions.mapAltitude}, + {m_xMax, m_yMax, insertionOptions.mapAltitude}}; + } + /** Scales an integer representation of the log-odd into a real valued * probability in [0,1], using p=exp(l)/(1+exp(l)) */ static inline float l2p(const cellType l) diff --git a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h index 0710d9d540..aadbf50f96 100644 --- a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h +++ b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h @@ -206,6 +206,15 @@ class COccupancyGridMap3D void getVisualizationInto( mrpt::opengl::CSetOfObjects& outObj) const override; + mrpt::math::TBoundingBoxf boundingBox() const override + { + return { + mrpt::math::TPoint3Df( + m_grid.getXMin(), m_grid.getYMin(), m_grid.getZMin()), + mrpt::math::TPoint3Df( + m_grid.getXMax(), m_grid.getYMax(), m_grid.getZMax())}; + } + /** With this struct options are provided to the observation insertion * process. * \sa CObservation::insertIntoGridMap */ diff --git a/libs/maps/include/mrpt/maps/CPointsMap.h b/libs/maps/include/mrpt/maps/CPointsMap.h index 656bf688ff..cdc519584f 100644 --- a/libs/maps/include/mrpt/maps/CPointsMap.h +++ b/libs/maps/include/mrpt/maps/CPointsMap.h @@ -931,7 +931,7 @@ class CPointsMap : public CMetricMap, * Results are cached unless the map is somehow modified to avoid repeated * calculations. */ - mrpt::math::TBoundingBoxf boundingBox() const; + mrpt::math::TBoundingBoxf boundingBox() const override; /** Extracts the points in the map within a cylinder in 3D defined the * provided radius and zmin/zmax values. diff --git a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h index 655c9e9a24..b38e06a576 100644 --- a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h +++ b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h @@ -16,6 +16,7 @@ #include #include #include +#include #include namespace mrpt::maps @@ -174,7 +175,7 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase, /** This visits all cells to calculate a bounding box, caching the result * so subsequent calls are cheap until the voxelmap is changed in some way. */ - mrpt::math::TBoundingBox getBoundingBox() const; + mrpt::math::TBoundingBoxf boundingBox() const override; /// The options used when inserting observations in the map: TVoxelMap_InsertionOptions insertionOptions; @@ -346,7 +347,7 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase, void updateCachedProperties() const; mutable mrpt::maps::CSimplePointsMap::Ptr m_cachedOccupied; - mutable mrpt::math::TBoundingBox m_bbox; + mutable mrpt::math::TBoundingBoxf m_bbox; }; // ============= Implementations =============== @@ -377,7 +378,7 @@ void CVoxelMapOccupancyBase::getAsOctoMapVoxels( auto& grid = const_cast&>(base_t::m_impl->grid); - const mrpt::math::TBoundingBox bbox = this->getBoundingBox(); + const mrpt::math::TBoundingBoxf bbox = this->boundingBox(); double bbox_span_z = bbox.max.z - bbox.min.z; if (bbox_span_z < 0) bbox_span_z = 1; const double bbox_span_z_inv = 1.0 / bbox_span_z; @@ -675,7 +676,7 @@ void CVoxelMapOccupancyBase::updateCachedProperties() if (m_cachedOccupied) return; // done m_cachedOccupied = mrpt::maps::CSimplePointsMap::Create(); - m_bbox = mrpt::math::TBoundingBox::PlusMinusInfinity(); + m_bbox = mrpt::math::TBoundingBoxf::PlusMinusInfinity(); // forEachCell() has no const version auto& grid = @@ -703,7 +704,7 @@ void CVoxelMapOccupancyBase::updateCachedProperties() grid.forEachCell(lmbdPerVoxel); // If no cell is active, use default bbox: - if (m_bbox == mrpt::math::TBoundingBox::PlusMinusInfinity()) m_bbox = {}; + if (m_bbox == mrpt::math::TBoundingBoxf::PlusMinusInfinity()) m_bbox = {}; } template @@ -715,8 +716,8 @@ mrpt::maps::CSimplePointsMap::Ptr } template -mrpt::math::TBoundingBox - CVoxelMapOccupancyBase::getBoundingBox() const +mrpt::math::TBoundingBoxf + CVoxelMapOccupancyBase::boundingBox() const { updateCachedProperties(); return m_bbox; diff --git a/libs/obs/include/mrpt/maps/CMetricMap.h b/libs/obs/include/mrpt/maps/CMetricMap.h index 4d85dca760..2341f4fb09 100644 --- a/libs/obs/include/mrpt/maps/CMetricMap.h +++ b/libs/obs/include/mrpt/maps/CMetricMap.h @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -113,6 +114,15 @@ class CMetricMap : public mrpt::serialization::CSerializable, */ virtual bool isEmpty() const = 0; + /** Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the + * default value of mrpt::math::TBoundingBoxf() if not implemented in the + * derived class or the map is empty. + */ + virtual auto boundingBox() const -> mrpt::math::TBoundingBoxf + { + return {}; // default impl: no bbox + } + /** Load the map contents from a CSimpleMap object, erasing all previous * content of the map. This is done invoking `insertObservation()` for each * observation at the mean 3D robot pose of each pose-observations pair in diff --git a/libs/opengl/src/CPointCloud.cpp b/libs/opengl/src/CPointCloud.cpp index 8260ca2fb8..0aa9fd3b25 100644 --- a/libs/opengl/src/CPointCloud.cpp +++ b/libs/opengl/src/CPointCloud.cpp @@ -491,5 +491,4 @@ void CPointCloud::toYAMLMap(mrpt::containers::yaml& propertiesMap) const { CRenderizable::toYAMLMap(propertiesMap); propertiesMap["point_count"] = m_points.size(); - propertiesMap["bounding_box"] = this->getBoundingBox().asString(); } From 2cbd5dc51111e43d5232b9e0fe2a491744818497 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 5 Nov 2023 01:46:46 +0100 Subject: [PATCH 11/36] Fix build of constexpr expressions with points --- libs/math/include/mrpt/math/TPoint2D.h | 4 ++-- libs/math/include/mrpt/math/TPoint3D.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libs/math/include/mrpt/math/TPoint2D.h b/libs/math/include/mrpt/math/TPoint2D.h index 6a4bdcc831..f1d7baf49c 100644 --- a/libs/math/include/mrpt/math/TPoint2D.h +++ b/libs/math/include/mrpt/math/TPoint2D.h @@ -25,8 +25,8 @@ namespace mrpt::math template struct TPoint2D_data { - TPoint2D_data() = default; - TPoint2D_data(T X, T Y) : x(X), y(Y) {} + constexpr TPoint2D_data() = default; + constexpr TPoint2D_data(T X, T Y) : x(X), y(Y) {} /** X,Y coordinates */ T x, y; diff --git a/libs/math/include/mrpt/math/TPoint3D.h b/libs/math/include/mrpt/math/TPoint3D.h index ba730b091c..2314274879 100644 --- a/libs/math/include/mrpt/math/TPoint3D.h +++ b/libs/math/include/mrpt/math/TPoint3D.h @@ -30,8 +30,8 @@ namespace mrpt::math template struct TPoint3D_data { - TPoint3D_data() = default; - TPoint3D_data(T X, T Y, T Z) : x(X), y(Y), z(Z) {} + constexpr TPoint3D_data() = default; + constexpr TPoint3D_data(T X, T Y, T Z) : x(X), y(Y), z(Z) {} /** X,Y,Z coordinates */ T x, y, z; From 9c3ec311a56bc1e0827034e6512f0444032a3a5e Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 5 Nov 2023 01:52:27 +0100 Subject: [PATCH 12/36] Fix bbox type narrowing warnings --- libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h index b38e06a576..540dc2b99f 100644 --- a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h +++ b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h @@ -347,7 +347,7 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase, void updateCachedProperties() const; mutable mrpt::maps::CSimplePointsMap::Ptr m_cachedOccupied; - mutable mrpt::math::TBoundingBoxf m_bbox; + mutable mrpt::math::TBoundingBox m_bbox; }; // ============= Implementations =============== @@ -676,7 +676,7 @@ void CVoxelMapOccupancyBase::updateCachedProperties() if (m_cachedOccupied) return; // done m_cachedOccupied = mrpt::maps::CSimplePointsMap::Create(); - m_bbox = mrpt::math::TBoundingBoxf::PlusMinusInfinity(); + m_bbox = mrpt::math::TBoundingBox::PlusMinusInfinity(); // forEachCell() has no const version auto& grid = @@ -704,7 +704,7 @@ void CVoxelMapOccupancyBase::updateCachedProperties() grid.forEachCell(lmbdPerVoxel); // If no cell is active, use default bbox: - if (m_bbox == mrpt::math::TBoundingBoxf::PlusMinusInfinity()) m_bbox = {}; + if (m_bbox == mrpt::math::TBoundingBox::PlusMinusInfinity()) m_bbox = {}; } template @@ -720,7 +720,7 @@ mrpt::math::TBoundingBoxf CVoxelMapOccupancyBase::boundingBox() const { updateCachedProperties(); - return m_bbox; + return {m_bbox.min.cast(), m_bbox.max.cast()}; } } // namespace mrpt::maps From 54d1f848df2762904628b0f8826e0eb81c97c255 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 5 Nov 2023 01:56:41 +0100 Subject: [PATCH 13/36] Update python wrapper --- python/all_mrpt_maps1.cpp | 1 + python/src/mrpt/bayes/CParticleFilterData.cpp | 1 + .../src/mrpt/bayes/CProbabilityParticle.cpp | 1 + python/src/mrpt/core/bits_math_1.cpp | 6 +- python/src/mrpt/maps/CBeacon.cpp | 13 +++ python/src/mrpt/maps/CColouredOctoMap.cpp | 52 +++++++++++ .../mrpt/maps/CGasConcentrationGridMap2D.cpp | 13 +++ .../src/mrpt/maps/CHeightGridMap2D_Base.cpp | 13 +++ python/src/mrpt/maps/CHeightGridMap2D_MRF.cpp | 13 +++ python/src/mrpt/maps/CLandmarksMap.cpp | 13 +++ python/src/mrpt/maps/CLogOddsGridMap3D.cpp | 1 + python/src/mrpt/maps/CMetricMap.cpp | 3 +- python/src/mrpt/maps/CMetricMapEvents.cpp | 1 + python/src/mrpt/maps/CMetricMap_1.cpp | 4 +- python/src/mrpt/maps/CMultiMetricMap.cpp | 13 +++ python/src/mrpt/maps/CMultiMetricMapPDF.cpp | 1 + python/src/mrpt/maps/COccupancyGridMap2D.cpp | 68 +++++++++++--- python/src/mrpt/maps/COccupancyGridMap3D.cpp | 65 +++++++++++--- python/src/mrpt/maps/COctoMap.cpp | 52 +++++++++++ python/src/mrpt/maps/COctoMapBase.cpp | 2 + python/src/mrpt/maps/COctoMapBase_1.cpp | 2 + .../mrpt/maps/CPointCloudFilterByDistance.cpp | 39 ++++++++ python/src/mrpt/maps/CPointsMap.cpp | 18 ++-- python/src/mrpt/maps/CPointsMap_1.cpp | 2 +- .../src/mrpt/maps/CReflectivityGridMap2D.cpp | 52 +++++++++++ python/src/mrpt/maps/CVoxelMap.cpp | 85 +++++++++++++++++- python/src/mrpt/maps/CVoxelMapBase.cpp | 3 + .../src/mrpt/maps/CVoxelMapOccupancyBase.cpp | 10 +-- .../mrpt/maps/CVoxelMapOccupancyBase_1.cpp | 14 ++- .../mrpt/maps/CVoxelMapOccupancyBase_2.cpp | 14 ++- .../src/mrpt/maps/CWirelessPowerGridMap2D.cpp | 13 +++ .../src/mrpt/maps/NearestNeighborsCapable.cpp | 33 +++++++ python/src/mrpt/math/TBoundingBox.cpp | 2 + python/src/mrpt/math/TPoint2D.cpp | 6 +- python/src/mrpt/math/TPoint3D.cpp | 20 +++-- python/src/mrpt/math/TPoint3D_1.cpp | 2 +- python/src/mrpt/math/TPoseOrPoint.cpp | 8 +- python/src/mrpt/obs/CObservationRange.cpp | 1 + python/src/mrpt/obs/CSensoryFrame.cpp | 1 + python/src/mrpt/opengl/COctoMapVoxels.cpp | 15 ++-- python/src/mrpt/poses/CPointPDF.cpp | 1 + python/src/mrpt/poses/CPose3DPDFParticles.cpp | 1 + python/src/mrpt/rtti/CObject.cpp | 1 + .../slam/CMetricMapsAlignmentAlgorithm.cpp | 1 + .../mrpt/slam/CMonteCarloLocalization3D.cpp | 1 + .../COccupancyGridMapFeatureExtractor.cpp | 2 + ...RejectionSamplingRangeOnlyLocalization.cpp | 1 + python/src/mrpt/slam/TKLDParams.cpp | 1 + python/src/pymrpt.cpp | 6 +- python/src/pymrpt.sources | 3 +- python/src/unknown/unknown_6.cpp | 12 +-- python/src/unknown/unknown_7.cpp | 16 ++-- python/src/unknown/unknown_8.cpp | 12 +-- python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi | 89 +++++++++++++------ python/stubs-out/mrpt/pymrpt/mrpt/math.pyi | 28 ++++-- .../mrpt/pymrpt/mrpt/obs/__init__.pyi | 1 + .../mrpt/pymrpt/mrpt/opengl/__init__.pyi | 6 ++ 57 files changed, 727 insertions(+), 131 deletions(-) create mode 100644 python/src/mrpt/maps/NearestNeighborsCapable.cpp diff --git a/python/all_mrpt_maps1.cpp b/python/all_mrpt_maps1.cpp index 5c4dd42233..0f58b9999e 100644 --- a/python/all_mrpt_maps1.cpp +++ b/python/all_mrpt_maps1.cpp @@ -9,3 +9,4 @@ #include "src/mrpt/maps/CLandmarksMap.cpp" #include "src/mrpt/maps/CLogOddsGridMap3D.cpp" #include "src/mrpt/maps/CLogOddsGridMapLUT.cpp" +#include "src/mrpt/maps/NearestNeighborsCapable.cpp" \ No newline at end of file diff --git a/python/src/mrpt/bayes/CParticleFilterData.cpp b/python/src/mrpt/bayes/CParticleFilterData.cpp index 42b31e132c..f21023ff2b 100644 --- a/python/src/mrpt/bayes/CParticleFilterData.cpp +++ b/python/src/mrpt/bayes/CParticleFilterData.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/mrpt/bayes/CProbabilityParticle.cpp b/python/src/mrpt/bayes/CProbabilityParticle.cpp index e251f86b8c..a456202bac 100644 --- a/python/src/mrpt/bayes/CProbabilityParticle.cpp +++ b/python/src/mrpt/bayes/CProbabilityParticle.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/mrpt/core/bits_math_1.cpp b/python/src/mrpt/core/bits_math_1.cpp index cf7759f7bb..b9e95d3904 100644 --- a/python/src/mrpt/core/bits_math_1.cpp +++ b/python/src/mrpt/core/bits_math_1.cpp @@ -39,13 +39,13 @@ void bind_mrpt_core_bits_math_1(std::function< pybind11::module &(std::string co // mrpt::keep_max(double &, const float) file:mrpt/core/bits_math.h line:149 M("mrpt").def("keep_max", (void (*)(double &, const float)) &mrpt::keep_max, "C++: mrpt::keep_max(double &, const float) --> void", pybind11::arg("var"), pybind11::arg("test_val")); - // mrpt::d2f(const double) file:mrpt/core/bits_math.h line:186 + // mrpt::d2f(const double) file:mrpt/core/bits_math.h line:187 M("mrpt").def("d2f", (float (*)(const double)) &mrpt::d2f, "shortcut for static_cast(double) \n\nC++: mrpt::d2f(const double) --> float", pybind11::arg("d")); - // mrpt::f2u8(const float) file:mrpt/core/bits_math.h line:190 + // mrpt::f2u8(const float) file:mrpt/core/bits_math.h line:191 M("mrpt").def("f2u8", (uint8_t (*)(const float)) &mrpt::f2u8, "converts a float [0,1] into an uint8_t [0,255] (without checking for out of\n bounds) \n\n u8tof \n\nC++: mrpt::f2u8(const float) --> uint8_t", pybind11::arg("f")); - // mrpt::u8tof(const unsigned char) file:mrpt/core/bits_math.h line:193 + // mrpt::u8tof(const unsigned char) file:mrpt/core/bits_math.h line:197 M("mrpt").def("u8tof", (float (*)(const unsigned char)) &mrpt::u8tof, "converts a uint8_t [0,255] into a float [0,1] \n f2u8 \n\nC++: mrpt::u8tof(const unsigned char) --> float", pybind11::arg("v")); } diff --git a/python/src/mrpt/maps/CBeacon.cpp b/python/src/mrpt/maps/CBeacon.cpp index a696a8ccab..fe6deb1367 100644 --- a/python/src/mrpt/maps/CBeacon.cpp +++ b/python/src/mrpt/maps/CBeacon.cpp @@ -454,6 +454,19 @@ struct PyCallBack_mrpt_maps_CBeaconMap : public mrpt::maps::CBeaconMap { } return CBeaconMap::getVisualizationInto(a0); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CMetricMap::boundingBox(); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); diff --git a/python/src/mrpt/maps/CColouredOctoMap.cpp b/python/src/mrpt/maps/CColouredOctoMap.cpp index 42867d818b..6773d3290e 100644 --- a/python/src/mrpt/maps/CColouredOctoMap.cpp +++ b/python/src/mrpt/maps/CColouredOctoMap.cpp @@ -441,6 +441,19 @@ struct PyCallBack_mrpt_maps_CColouredOctoMap : public mrpt::maps::CColouredOctoM } return COctoMapBase::getVisualizationInto(a0); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CMetricMap::boundingBox(); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); @@ -934,6 +947,19 @@ struct PyCallBack_mrpt_maps_CColouredPointsMap : public mrpt::maps::CColouredPoi } return CPointsMap::isEmpty(); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CPointsMap::boundingBox(); + } double internal_computeObservationLikelihood(const class mrpt::obs::CObservation & a0, const class mrpt::poses::CPose3D & a1) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "internal_computeObservationLikelihood"); @@ -960,6 +986,32 @@ struct PyCallBack_mrpt_maps_CColouredPointsMap : public mrpt::maps::CColouredPoi } return CPointsMap::asString(); } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2); + } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "PLY_import_set_face_count"); diff --git a/python/src/mrpt/maps/CGasConcentrationGridMap2D.cpp b/python/src/mrpt/maps/CGasConcentrationGridMap2D.cpp index 2ec1a9c41b..8cb788b707 100644 --- a/python/src/mrpt/maps/CGasConcentrationGridMap2D.cpp +++ b/python/src/mrpt/maps/CGasConcentrationGridMap2D.cpp @@ -396,6 +396,19 @@ struct PyCallBack_mrpt_maps_CGasConcentrationGridMap2D : public mrpt::maps::CGas } return CRandomFieldGridMap2D::predictMeasurement(a0, a1, a2, a3, a4, a5); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CMetricMap::boundingBox(); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); diff --git a/python/src/mrpt/maps/CHeightGridMap2D_Base.cpp b/python/src/mrpt/maps/CHeightGridMap2D_Base.cpp index 6ebb9daf84..b398b02b0c 100644 --- a/python/src/mrpt/maps/CHeightGridMap2D_Base.cpp +++ b/python/src/mrpt/maps/CHeightGridMap2D_Base.cpp @@ -458,6 +458,19 @@ struct PyCallBack_mrpt_maps_CHeightGridMap2D : public mrpt::maps::CHeightGridMap } return CHeightGridMap2D::internal_computeObservationLikelihood(a0, a1); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CMetricMap::boundingBox(); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); diff --git a/python/src/mrpt/maps/CHeightGridMap2D_MRF.cpp b/python/src/mrpt/maps/CHeightGridMap2D_MRF.cpp index 2c9a4d15e0..12c3dbf64b 100644 --- a/python/src/mrpt/maps/CHeightGridMap2D_MRF.cpp +++ b/python/src/mrpt/maps/CHeightGridMap2D_MRF.cpp @@ -475,6 +475,19 @@ struct PyCallBack_mrpt_maps_CHeightGridMap2D_MRF : public mrpt::maps::CHeightGri } return CRandomFieldGridMap2D::predictMeasurement(a0, a1, a2, a3, a4, a5); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CMetricMap::boundingBox(); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); diff --git a/python/src/mrpt/maps/CLandmarksMap.cpp b/python/src/mrpt/maps/CLandmarksMap.cpp index 18577cb9f8..9bd68adfab 100644 --- a/python/src/mrpt/maps/CLandmarksMap.cpp +++ b/python/src/mrpt/maps/CLandmarksMap.cpp @@ -259,6 +259,19 @@ struct PyCallBack_mrpt_maps_CLandmarksMap : public mrpt::maps::CLandmarksMap { } return CLandmarksMap::auxParticleFilterCleanUp(); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CMetricMap::boundingBox(); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); diff --git a/python/src/mrpt/maps/CLogOddsGridMap3D.cpp b/python/src/mrpt/maps/CLogOddsGridMap3D.cpp index 3f5c29247f..0e15401f5c 100644 --- a/python/src/mrpt/maps/CLogOddsGridMap3D.cpp +++ b/python/src/mrpt/maps/CLogOddsGridMap3D.cpp @@ -20,6 +20,7 @@ void bind_mrpt_maps_CLogOddsGridMap3D(std::function< pybind11::module &(std::str { // mrpt::maps::CLogOddsGridMap3D file:mrpt/maps/CLogOddsGridMap3D.h line:27 pybind11::class_, std::shared_ptr>> cl(M("mrpt::maps"), "CLogOddsGridMap3D_signed_char_t", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::CLogOddsGridMap3D(); } ) ); + cl.def( pybind11::init( [](mrpt::maps::CLogOddsGridMap3D const &o){ return new mrpt::maps::CLogOddsGridMap3D(o); } ) ); cl.def_readwrite("m_grid", &mrpt::maps::CLogOddsGridMap3D::m_grid); cl.def("updateCell_fast_occupied", (void (mrpt::maps::CLogOddsGridMap3D::*)(signed char *, const signed char, const signed char)) &mrpt::maps::CLogOddsGridMap3D::updateCell_fast_occupied, "C++: mrpt::maps::CLogOddsGridMap3D::updateCell_fast_occupied(signed char *, const signed char, const signed char) --> void", pybind11::arg("theCell"), pybind11::arg("logodd_obs"), pybind11::arg("thres")); cl.def("updateCell_fast_occupied", (void (mrpt::maps::CLogOddsGridMap3D::*)(const unsigned int, const unsigned int, const unsigned int, const signed char, const signed char)) &mrpt::maps::CLogOddsGridMap3D::updateCell_fast_occupied, "C++: mrpt::maps::CLogOddsGridMap3D::updateCell_fast_occupied(const unsigned int, const unsigned int, const unsigned int, const signed char, const signed char) --> void", pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("logodd_obs"), pybind11::arg("thres")); diff --git a/python/src/mrpt/maps/CMetricMap.cpp b/python/src/mrpt/maps/CMetricMap.cpp index 4de20ff394..8501ebb7df 100644 --- a/python/src/mrpt/maps/CMetricMap.cpp +++ b/python/src/mrpt/maps/CMetricMap.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -57,7 +58,7 @@ void bind_mrpt_maps_CMetricMap(std::function< pybind11::module &(std::string const &namespace_) > &M) { - // mrpt::pymrpt_internal::insertObs(const class mrpt::obs::CSensoryFrame &, class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D *) file:mrpt/maps/CMetricMap.h line:36 + // mrpt::pymrpt_internal::insertObs(const class mrpt::obs::CSensoryFrame &, class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D *) file:mrpt/maps/CMetricMap.h line:37 M("mrpt::pymrpt_internal").def("insertObs", (bool (*)(const class mrpt::obs::CSensoryFrame &, class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D *)) &mrpt::pymrpt_internal::insertObs, "C++: mrpt::pymrpt_internal::insertObs(const class mrpt::obs::CSensoryFrame &, class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("sf"), pybind11::arg("map"), pybind11::arg("robotPose")); } diff --git a/python/src/mrpt/maps/CMetricMapEvents.cpp b/python/src/mrpt/maps/CMetricMapEvents.cpp index f305bde7ce..cd9499b267 100644 --- a/python/src/mrpt/maps/CMetricMapEvents.cpp +++ b/python/src/mrpt/maps/CMetricMapEvents.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/mrpt/maps/CMetricMap_1.cpp b/python/src/mrpt/maps/CMetricMap_1.cpp index 98651090df..e978d10307 100644 --- a/python/src/mrpt/maps/CMetricMap_1.cpp +++ b/python/src/mrpt/maps/CMetricMap_1.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -54,13 +55,14 @@ void bind_mrpt_maps_CMetricMap_1(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::CMetricMap file:mrpt/maps/CMetricMap.h line:71 + { // mrpt::maps::CMetricMap file:mrpt/maps/CMetricMap.h line:72 pybind11::class_, mrpt::serialization::CSerializable, mrpt::system::CObservable, mrpt::Stringifyable, mrpt::opengl::Visualizable> cl(M("mrpt::maps"), "CMetricMap", "Declares a virtual base class for all metric maps storage classes.\n In this class virtual methods are provided to allow the insertion\n of any type of \"CObservation\" objects into the metric map, thus\n updating the map (doesn't matter if it is a 2D/3D grid, a point map, etc.).\n\n Observations don't include any information about the\n robot pose, just the raw observation and information about\n the sensor pose relative to the robot mobile base coordinates origin.\n\n Note that all metric maps implement this mrpt::system::CObservable\ninterface,\n emitting the following events:\n - mrpt::obs::mrptEventMetricMapClear: Upon call of the ::clear() method.\n - mrpt::obs::mrptEventMetricMapInsert: Upon insertion of an observation\nthat effectively modifies the map (e.g. inserting an image into a grid map\nwill NOT raise an event, inserting a laser scan will).\n\n To check what observations are supported by each metric map, see\n \n\n \n All derived class must implement a static class factory\n`::MapDefinition()` that builds a default\nTMetricMapInitializer\n\n \n CObservation, CSensoryFrame, CMultiMetricMap\n \n\n\n "); cl.def_readwrite("genericMapParams", &mrpt::maps::CMetricMap::genericMapParams); cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::GetRuntimeClass, "C++: mrpt::maps::CMetricMap::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CMetricMap::GetRuntimeClassIdStatic, "C++: mrpt::maps::CMetricMap::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); + cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::boundingBox, "Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the\n default value of mrpt::math::TBoundingBoxf() if not implemented in the\n derived class or the map is empty.\n\nC++: mrpt::maps::CMetricMap::boundingBox() const --> struct mrpt::math::TBoundingBox_"); cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); diff --git a/python/src/mrpt/maps/CMultiMetricMap.cpp b/python/src/mrpt/maps/CMultiMetricMap.cpp index e620ace65d..b97a5246d0 100644 --- a/python/src/mrpt/maps/CMultiMetricMap.cpp +++ b/python/src/mrpt/maps/CMultiMetricMap.cpp @@ -307,6 +307,19 @@ struct PyCallBack_mrpt_maps_CMultiMetricMap : public mrpt::maps::CMultiMetricMap } return CMultiMetricMap::internal_computeObservationLikelihood(a0, a1); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CMetricMap::boundingBox(); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); diff --git a/python/src/mrpt/maps/CMultiMetricMapPDF.cpp b/python/src/mrpt/maps/CMultiMetricMapPDF.cpp index d7cd34c3eb..e6b7d92b80 100644 --- a/python/src/mrpt/maps/CMultiMetricMapPDF.cpp +++ b/python/src/mrpt/maps/CMultiMetricMapPDF.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/mrpt/maps/COccupancyGridMap2D.cpp b/python/src/mrpt/maps/COccupancyGridMap2D.cpp index 6d67e0b14b..b6bdbc90a7 100644 --- a/python/src/mrpt/maps/COccupancyGridMap2D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap2D.cpp @@ -97,7 +97,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::maps::COccupancyGridMap2D file:mrpt/maps/COccupancyGridMap2D.h line:53 +// mrpt::maps::COccupancyGridMap2D file:mrpt/maps/COccupancyGridMap2D.h line:54 struct PyCallBack_mrpt_maps_COccupancyGridMap2D : public mrpt::maps::COccupancyGridMap2D { using mrpt::maps::COccupancyGridMap2D::COccupancyGridMap2D; @@ -192,6 +192,19 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap2D : public mrpt::maps::COccupancyG } return COccupancyGridMap2D::internal_clear(); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return COccupancyGridMap2D::boundingBox(); + } void getVisualizationInto(class mrpt::opengl::CSetOfObjects & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "getVisualizationInto"); @@ -270,6 +283,32 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap2D : public mrpt::maps::COccupancyG } return COccupancyGridMap2D::asString(); } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return COccupancyGridMap2D::nn_single_search(a0, a1, a2); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return COccupancyGridMap2D::nn_single_search(a0, a1, a2); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); @@ -324,7 +363,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap2D : public mrpt::maps::COccupancyG } }; -// mrpt::maps::COccupancyGridMap2D::TInsertionOptions file:mrpt/maps/COccupancyGridMap2D.h line:466 +// mrpt::maps::COccupancyGridMap2D::TInsertionOptions file:mrpt/maps/COccupancyGridMap2D.h line:475 struct PyCallBack_mrpt_maps_COccupancyGridMap2D_TInsertionOptions : public mrpt::maps::COccupancyGridMap2D::TInsertionOptions { using mrpt::maps::COccupancyGridMap2D::TInsertionOptions::TInsertionOptions; @@ -356,7 +395,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap2D_TInsertionOptions : public mrpt: } }; -// mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions file:mrpt/maps/COccupancyGridMap2D.h line:547 +// mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions file:mrpt/maps/COccupancyGridMap2D.h line:556 struct PyCallBack_mrpt_maps_COccupancyGridMap2D_TLikelihoodOptions : public mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions { using mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions::TLikelihoodOptions; @@ -435,8 +474,8 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap2D_TMapDefinition : public mrpt::ma void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::COccupancyGridMap2D file:mrpt/maps/COccupancyGridMap2D.h line:53 - pybind11::class_, PyCallBack_mrpt_maps_COccupancyGridMap2D, mrpt::maps::CMetricMap, mrpt::maps::CLogOddsGridMap2D> cl(M("mrpt::maps"), "COccupancyGridMap2D", "A class for storing an occupancy grid map.\n COccupancyGridMap2D is a class for storing a metric map\n representation in the form of a probabilistic occupancy\n grid map: value of 0 means certainly occupied, 1 means\n a certainly empty cell. Initially 0.5 means uncertainty.\n\n The cells keep the log-odd representation of probabilities instead of the\nprobabilities themselves.\n More details can be found at https://www.mrpt.org/Occupancy_Grids\n\n The algorithm for updating the grid from a laser scanner can optionally take\ninto account the progressive widening of the beams, as\n described in [this page](http://www.mrpt.org/Occupancy_Grids)\n\n Some implemented methods are:\n - Update of individual cells\n - Insertion of observations\n - Voronoi diagram and critical points (\n - Saving and loading from/to a bitmap\n - Laser scans simulation for the map contents\n - Entropy and information methods (See computeEntropy)\n\n "); + { // mrpt::maps::COccupancyGridMap2D file:mrpt/maps/COccupancyGridMap2D.h line:54 + pybind11::class_, PyCallBack_mrpt_maps_COccupancyGridMap2D, mrpt::maps::CMetricMap, mrpt::maps::CLogOddsGridMap2D, mrpt::maps::NearestNeighborsCapable> cl(M("mrpt::maps"), "COccupancyGridMap2D", "A class for storing an occupancy grid map.\n COccupancyGridMap2D is a class for storing a metric map\n representation in the form of a probabilistic occupancy\n grid map: value of 0 means certainly occupied, 1 means\n a certainly empty cell. Initially 0.5 means uncertainty.\n\n The cells keep the log-odd representation of probabilities instead of the\nprobabilities themselves.\n More details can be found at https://www.mrpt.org/Occupancy_Grids\n\n The algorithm for updating the grid from a laser scanner can optionally take\ninto account the progressive widening of the beams, as\n described in [this page](http://www.mrpt.org/Occupancy_Grids)\n\n Some implemented methods are:\n - Update of individual cells\n - Insertion of observations\n - Voronoi diagram and critical points (\n - Saving and loading from/to a bitmap\n - Laser scans simulation for the map contents\n - Entropy and information methods (See computeEntropy)\n\n "); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap2D(); }, [](){ return new PyCallBack_mrpt_maps_COccupancyGridMap2D(); } ), "doc"); cl.def( pybind11::init( [](float const & a0){ return new mrpt::maps::COccupancyGridMap2D(a0); }, [](float const & a0){ return new PyCallBack_mrpt_maps_COccupancyGridMap2D(a0); } ), "doc"); cl.def( pybind11::init( [](float const & a0, float const & a1){ return new mrpt::maps::COccupancyGridMap2D(a0, a1); }, [](float const & a0, float const & a1){ return new PyCallBack_mrpt_maps_COccupancyGridMap2D(a0, a1); } ), "doc"); @@ -496,6 +535,7 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s cl.def("idx2y", (float (mrpt::maps::COccupancyGridMap2D::*)(size_t) const) &mrpt::maps::COccupancyGridMap2D::idx2y, "C++: mrpt::maps::COccupancyGridMap2D::idx2y(size_t) const --> float", pybind11::arg("cy")); cl.def("x2idx", (int (mrpt::maps::COccupancyGridMap2D::*)(float, float) const) &mrpt::maps::COccupancyGridMap2D::x2idx, "Transform a coordinate value into a cell index, using a diferent \"x_min\"\n value \n\nC++: mrpt::maps::COccupancyGridMap2D::x2idx(float, float) const --> int", pybind11::arg("x"), pybind11::arg("xmin")); cl.def("y2idx", (int (mrpt::maps::COccupancyGridMap2D::*)(float, float) const) &mrpt::maps::COccupancyGridMap2D::y2idx, "C++: mrpt::maps::COccupancyGridMap2D::y2idx(float, float) const --> int", pybind11::arg("y"), pybind11::arg("ymin")); + cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::boundingBox, "C++: mrpt::maps::COccupancyGridMap2D::boundingBox() const --> struct mrpt::math::TBoundingBox_"); cl.def_static("l2p", (float (*)(const signed char)) &mrpt::maps::COccupancyGridMap2D::l2p, "Scales an integer representation of the log-odd into a real valued\n probability in [0,1], using p=exp(l)/(1+exp(l)) \n\nC++: mrpt::maps::COccupancyGridMap2D::l2p(const signed char) --> float", pybind11::arg("l")); cl.def_static("l2p_255", (uint8_t (*)(const signed char)) &mrpt::maps::COccupancyGridMap2D::l2p_255, "Scales an integer representation of the log-odd into a linear scale\n [0,255], using p=exp(l)/(1+exp(l)) \n\nC++: mrpt::maps::COccupancyGridMap2D::l2p_255(const signed char) --> uint8_t", pybind11::arg("l")); cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::COccupancyGridMap2D::p2l, "Scales a real valued probability in [0,1] to an integer representation\n of: log(p)-log(1-p) in the valid range of cellType \n\nC++: mrpt::maps::COccupancyGridMap2D::p2l(const float) --> signed char", pybind11::arg("p")); @@ -569,9 +609,11 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s cl.def("compute3DMatchingRatio", (float (mrpt::maps::COccupancyGridMap2D::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const) &mrpt::maps::COccupancyGridMap2D::compute3DMatchingRatio, "See docs in base class: in this class this always returns 0 \n\nC++: mrpt::maps::COccupancyGridMap2D::compute3DMatchingRatio(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const --> float", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("params")); cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::COccupancyGridMap2D::*)(const std::string &) const) &mrpt::maps::COccupancyGridMap2D::saveMetricMapRepresentationToFile, "This virtual method saves the map to a file \"filNamePrefix\"+<\n some_file_extension >, as an image or in any other applicable way (Notice\n that other methods to save the map may be implemented in classes\n implementing this virtual interface). \n\nC++: mrpt::maps::COccupancyGridMap2D::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("filNamePrefix")); cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap2D::asString() const --> std::string"); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, " @{ \n\nC++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); cl.def("assign", (class mrpt::maps::COccupancyGridMap2D & (mrpt::maps::COccupancyGridMap2D::*)(const class mrpt::maps::COccupancyGridMap2D &)) &mrpt::maps::COccupancyGridMap2D::operator=, "C++: mrpt::maps::COccupancyGridMap2D::operator=(const class mrpt::maps::COccupancyGridMap2D &) --> class mrpt::maps::COccupancyGridMap2D &", pybind11::return_value_policy::automatic, pybind11::arg("")); - { // mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly file:mrpt/maps/COccupancyGridMap2D.h line:213 + { // mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly file:mrpt/maps/COccupancyGridMap2D.h line:215 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TUpdateCellsInfoChangeOnly", "An internal structure for storing data related to counting the new\n information apported by some observation "); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly(); } ) ); @@ -583,7 +625,7 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s cl.def("assign", (struct mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly & (mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly::*)(const struct mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly &)) &mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly::operator=, "C++: mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly::operator=(const struct mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly &) --> struct mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::COccupancyGridMap2D::TEntropyInfo file:mrpt/maps/COccupancyGridMap2D.h line:441 + { // mrpt::maps::COccupancyGridMap2D::TEntropyInfo file:mrpt/maps/COccupancyGridMap2D.h line:450 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TEntropyInfo", "Used for returning entropy related information \n computeEntropy "); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap2D::TEntropyInfo(); } ) ); @@ -595,7 +637,7 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s cl.def_readwrite("effectiveMappedCells", &mrpt::maps::COccupancyGridMap2D::TEntropyInfo::effectiveMappedCells); } - { // mrpt::maps::COccupancyGridMap2D::TInsertionOptions file:mrpt/maps/COccupancyGridMap2D.h line:466 + { // mrpt::maps::COccupancyGridMap2D::TInsertionOptions file:mrpt/maps/COccupancyGridMap2D.h line:475 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_COccupancyGridMap2D_TInsertionOptions, mrpt::config::CLoadableOptions> cl(enclosing_class, "TInsertionOptions", "With this struct options are provided to the observation insertion\n process.\n \n\n CObservation::insertIntoGridMap "); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap2D::TInsertionOptions(); }, [](){ return new PyCallBack_mrpt_maps_COccupancyGridMap2D_TInsertionOptions(); } ) ); @@ -617,7 +659,7 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s cl.def("assign", (class mrpt::maps::COccupancyGridMap2D::TInsertionOptions & (mrpt::maps::COccupancyGridMap2D::TInsertionOptions::*)(const class mrpt::maps::COccupancyGridMap2D::TInsertionOptions &)) &mrpt::maps::COccupancyGridMap2D::TInsertionOptions::operator=, "C++: mrpt::maps::COccupancyGridMap2D::TInsertionOptions::operator=(const class mrpt::maps::COccupancyGridMap2D::TInsertionOptions &) --> class mrpt::maps::COccupancyGridMap2D::TInsertionOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions file:mrpt/maps/COccupancyGridMap2D.h line:547 + { // mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions file:mrpt/maps/COccupancyGridMap2D.h line:556 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_COccupancyGridMap2D_TLikelihoodOptions, mrpt::config::CLoadableOptions> cl(enclosing_class, "TLikelihoodOptions", "With this struct options are provided to the observation likelihood\n computation process "); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions(); }, [](){ return new PyCallBack_mrpt_maps_COccupancyGridMap2D_TLikelihoodOptions(); } ) ); @@ -646,7 +688,7 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s cl.def("assign", (class mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions & (mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions::*)(const class mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions &)) &mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions::operator=, "C++: mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions::operator=(const class mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions &) --> class mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::COccupancyGridMap2D::TLikelihoodOutput file:mrpt/maps/COccupancyGridMap2D.h line:633 + { // mrpt::maps::COccupancyGridMap2D::TLikelihoodOutput file:mrpt/maps/COccupancyGridMap2D.h line:642 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TLikelihoodOutput", "Some members of this struct will contain intermediate or output data\n after calling \"computeObservationLikelihood\" for some likelihood\n functions "); cl.def( pybind11::init( [](mrpt::maps::COccupancyGridMap2D::TLikelihoodOutput const &o){ return new mrpt::maps::COccupancyGridMap2D::TLikelihoodOutput(o); } ) ); @@ -656,7 +698,7 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s cl.def("assign", (struct mrpt::maps::COccupancyGridMap2D::TLikelihoodOutput & (mrpt::maps::COccupancyGridMap2D::TLikelihoodOutput::*)(const struct mrpt::maps::COccupancyGridMap2D::TLikelihoodOutput &)) &mrpt::maps::COccupancyGridMap2D::TLikelihoodOutput::operator=, "C++: mrpt::maps::COccupancyGridMap2D::TLikelihoodOutput::operator=(const struct mrpt::maps::COccupancyGridMap2D::TLikelihoodOutput &) --> struct mrpt::maps::COccupancyGridMap2D::TLikelihoodOutput &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyParams file:mrpt/maps/COccupancyGridMap2D.h line:842 + { // mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyParams file:mrpt/maps/COccupancyGridMap2D.h line:851 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TLaserSimulUncertaintyParams", "Input params for laserScanSimulatorWithUncertainty() "); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyParams(); } ) ); @@ -677,14 +719,14 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s cl.def_readwrite("threshold", &mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyParams::threshold); } - { // mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyResult file:mrpt/maps/COccupancyGridMap2D.h line:894 + { // mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyResult file:mrpt/maps/COccupancyGridMap2D.h line:903 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TLaserSimulUncertaintyResult", "Output params for laserScanSimulatorWithUncertainty() "); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyResult(); } ) ); cl.def_readwrite("scanWithUncert", &mrpt::maps::COccupancyGridMap2D::TLaserSimulUncertaintyResult::scanWithUncert); } - { // mrpt::maps::COccupancyGridMap2D::TCriticalPointsList file:mrpt/maps/COccupancyGridMap2D.h line:1135 + { // mrpt::maps::COccupancyGridMap2D::TCriticalPointsList file:mrpt/maps/COccupancyGridMap2D.h line:1144 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TCriticalPointsList", "The structure used to store the set of Voronoi diagram\n critical points.\n \n\n findCriticalPoints"); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap2D::TCriticalPointsList(); } ) ); diff --git a/python/src/mrpt/maps/COccupancyGridMap3D.cpp b/python/src/mrpt/maps/COccupancyGridMap3D.cpp index b95c368820..e1f5b4bd8a 100644 --- a/python/src/mrpt/maps/COccupancyGridMap3D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap3D.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -89,7 +90,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::maps::COccupancyGridMap3D file:mrpt/maps/COccupancyGridMap3D.h line:36 +// mrpt::maps::COccupancyGridMap3D file:mrpt/maps/COccupancyGridMap3D.h line:42 struct PyCallBack_mrpt_maps_COccupancyGridMap3D : public mrpt::maps::COccupancyGridMap3D { using mrpt::maps::COccupancyGridMap3D::COccupancyGridMap3D; @@ -197,6 +198,19 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D : public mrpt::maps::COccupancyG } return COccupancyGridMap3D::getVisualizationInto(a0); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return COccupancyGridMap3D::boundingBox(); + } bool isEmpty() const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "isEmpty"); @@ -262,6 +276,32 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D : public mrpt::maps::COccupancyG } return COccupancyGridMap3D::asString(); } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return COccupancyGridMap3D::nn_single_search(a0, a1, a2); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return COccupancyGridMap3D::nn_single_search(a0, a1, a2); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); @@ -316,7 +356,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D : public mrpt::maps::COccupancyG } }; -// mrpt::maps::COccupancyGridMap3D::TInsertionOptions file:mrpt/maps/COccupancyGridMap3D.h line:200 +// mrpt::maps::COccupancyGridMap3D::TInsertionOptions file:mrpt/maps/COccupancyGridMap3D.h line:221 struct PyCallBack_mrpt_maps_COccupancyGridMap3D_TInsertionOptions : public mrpt::maps::COccupancyGridMap3D::TInsertionOptions { using mrpt::maps::COccupancyGridMap3D::TInsertionOptions::TInsertionOptions; @@ -348,7 +388,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D_TInsertionOptions : public mrpt: } }; -// mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions file:mrpt/maps/COccupancyGridMap3D.h line:257 +// mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions file:mrpt/maps/COccupancyGridMap3D.h line:283 struct PyCallBack_mrpt_maps_COccupancyGridMap3D_TLikelihoodOptions : public mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions { using mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::TLikelihoodOptions; @@ -427,13 +467,15 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D_TMapDefinition : public mrpt::ma void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::COccupancyGridMap3D file:mrpt/maps/COccupancyGridMap3D.h line:36 - pybind11::class_, PyCallBack_mrpt_maps_COccupancyGridMap3D, mrpt::maps::CMetricMap, mrpt::maps::CLogOddsGridMap3D> cl(M("mrpt::maps"), "COccupancyGridMap3D", "A 3D occupancy grid map with a regular, even distribution of voxels.\n\n This is a faster alternative to COctoMap, but use with caution with limited\nmap extensions, since it could easily exaust available memory.\n\n Each voxel follows a Bernoulli probability distribution: a value of 0 means\ncertainly occupied, 1 means a certainly empty voxel. Initially 0.5 means\nuncertainty.\n\n An alternative, sparse representation of a 3D map is provided\n via mrpt::maps::CVoxelMap and mrpt::maps::CVoxelMapRGB\n\n "); + { // mrpt::maps::COccupancyGridMap3D file:mrpt/maps/COccupancyGridMap3D.h line:42 + pybind11::class_, PyCallBack_mrpt_maps_COccupancyGridMap3D, mrpt::maps::CMetricMap, mrpt::maps::CLogOddsGridMap3D, mrpt::maps::NearestNeighborsCapable> cl(M("mrpt::maps"), "COccupancyGridMap3D", "A 3D occupancy grid map with a regular, even distribution of voxels.\n\n This is a faster alternative to COctoMap, but use with caution with limited\nmap extensions, since it could easily exaust available memory.\n\n Each voxel follows a Bernoulli probability distribution: a value of 0 means\ncertainly occupied, 1 means a certainly empty voxel. Initially 0.5 means\nuncertainty.\n\n An alternative, sparse representation of a 3D map is provided\n via mrpt::maps::CVoxelMap and mrpt::maps::CVoxelMapRGB\n\n "); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap3D(); }, [](){ return new PyCallBack_mrpt_maps_COccupancyGridMap3D(); } ), "doc"); cl.def( pybind11::init( [](const struct mrpt::math::TPoint3D_ & a0){ return new mrpt::maps::COccupancyGridMap3D(a0); }, [](const struct mrpt::math::TPoint3D_ & a0){ return new PyCallBack_mrpt_maps_COccupancyGridMap3D(a0); } ), "doc"); cl.def( pybind11::init( [](const struct mrpt::math::TPoint3D_ & a0, const struct mrpt::math::TPoint3D_ & a1){ return new mrpt::maps::COccupancyGridMap3D(a0, a1); }, [](const struct mrpt::math::TPoint3D_ & a0, const struct mrpt::math::TPoint3D_ & a1){ return new PyCallBack_mrpt_maps_COccupancyGridMap3D(a0, a1); } ), "doc"); cl.def( pybind11::init &, const struct mrpt::math::TPoint3D_ &, float>(), pybind11::arg("corner_min"), pybind11::arg("corner_max"), pybind11::arg("resolution") ); + cl.def( pybind11::init( [](PyCallBack_mrpt_maps_COccupancyGridMap3D const &o){ return new PyCallBack_mrpt_maps_COccupancyGridMap3D(o); } ) ); + cl.def( pybind11::init( [](mrpt::maps::COccupancyGridMap3D const &o){ return new mrpt::maps::COccupancyGridMap3D(o); } ) ); pybind11::enum_(cl, "TLikelihoodMethod", pybind11::arithmetic(), "The type for selecting a likelihood computation method ") .value("lmLikelihoodField_Thrun", mrpt::maps::COccupancyGridMap3D::lmLikelihoodField_Thrun) @@ -463,18 +505,19 @@ void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::s cl.def("getFreenessByPos", (float (mrpt::maps::COccupancyGridMap3D::*)(float, float, float) const) &mrpt::maps::COccupancyGridMap3D::getFreenessByPos, "Read the real valued [0,1] contents of a voxel, given its coordinates \n\nC++: mrpt::maps::COccupancyGridMap3D::getFreenessByPos(float, float, float) const --> float", pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z")); cl.def("insertRay", [](mrpt::maps::COccupancyGridMap3D &o, const struct mrpt::math::TPoint3D_ & a0, const struct mrpt::math::TPoint3D_ & a1) -> void { return o.insertRay(a0, a1); }, "", pybind11::arg("sensor"), pybind11::arg("end")); cl.def("insertRay", (void (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint3D_ &, const struct mrpt::math::TPoint3D_ &, bool)) &mrpt::maps::COccupancyGridMap3D::insertRay, "Increases the freeness of a ray segment, and the occupancy of the voxel\n at its end point (unless endIsOccupied=false).\n Normally, users would prefer the higher-level method\n CMetricMap::insertObservation()\n\nC++: mrpt::maps::COccupancyGridMap3D::insertRay(const struct mrpt::math::TPoint3D_ &, const struct mrpt::math::TPoint3D_ &, bool) --> void", pybind11::arg("sensor"), pybind11::arg("end"), pybind11::arg("endIsOccupied")); - cl.def("insertPointCloud", [](mrpt::maps::COccupancyGridMap3D &o, const struct mrpt::math::TPoint3D_ & a0, const class mrpt::maps::CPointsMap & a1) -> void { return o.insertPointCloud(a0, a1); }, "", pybind11::arg("sensorCenter"), pybind11::arg("pts")); - cl.def("insertPointCloud", (void (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint3D_ &, const class mrpt::maps::CPointsMap &, const float)) &mrpt::maps::COccupancyGridMap3D::insertPointCloud, "Calls insertRay() for each point in the point cloud, using as sensor\n central point (the origin of all rays), the given `sensorCenter`.\n \n\n If a point has larger distance from\n `sensorCenter` than `maxValidRange`, it will be considered a non-echo,\n and NO occupied voxel will be created at the end of the segment.\n \n\n insertionOptions parameters are observed in this method.\n\nC++: mrpt::maps::COccupancyGridMap3D::insertPointCloud(const struct mrpt::math::TPoint3D_ &, const class mrpt::maps::CPointsMap &, const float) --> void", pybind11::arg("sensorCenter"), pybind11::arg("pts"), pybind11::arg("maxValidRange")); cl.def("getAsOctoMapVoxels", (void (mrpt::maps::COccupancyGridMap3D::*)(class mrpt::opengl::COctoMapVoxels &) const) &mrpt::maps::COccupancyGridMap3D::getAsOctoMapVoxels, "renderingOptions \n\nC++: mrpt::maps::COccupancyGridMap3D::getAsOctoMapVoxels(class mrpt::opengl::COctoMapVoxels &) const --> void", pybind11::arg("gl_obj")); cl.def("getVisualizationInto", (void (mrpt::maps::COccupancyGridMap3D::*)(class mrpt::opengl::CSetOfObjects &) const) &mrpt::maps::COccupancyGridMap3D::getVisualizationInto, "Returns a 3D object representing the map. \n renderingOptions \n\nC++: mrpt::maps::COccupancyGridMap3D::getVisualizationInto(class mrpt::opengl::CSetOfObjects &) const --> void", pybind11::arg("outObj")); + cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::boundingBox, "C++: mrpt::maps::COccupancyGridMap3D::boundingBox() const --> struct mrpt::math::TBoundingBox_"); cl.def("isEmpty", (bool (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::isEmpty, "Returns true upon map construction or after calling clear(), the return\n changes to false upon successful insertObservation() or any other\n method to load data in the map.\n\nC++: mrpt::maps::COccupancyGridMap3D::isEmpty() const --> bool"); cl.def("determineMatching2D", (void (mrpt::maps::COccupancyGridMap3D::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose2D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const) &mrpt::maps::COccupancyGridMap3D::determineMatching2D, "See docs in base class: in this class this always returns 0 \n\nC++: mrpt::maps::COccupancyGridMap3D::determineMatching2D(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose2D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const --> void", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("correspondences"), pybind11::arg("params"), pybind11::arg("extraResults")); cl.def("compute3DMatchingRatio", (float (mrpt::maps::COccupancyGridMap3D::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const) &mrpt::maps::COccupancyGridMap3D::compute3DMatchingRatio, "See docs in base class: in this class this always returns 0 \n\nC++: mrpt::maps::COccupancyGridMap3D::compute3DMatchingRatio(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const --> float", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("params")); cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::COccupancyGridMap3D::*)(const std::string &) const) &mrpt::maps::COccupancyGridMap3D::saveMetricMapRepresentationToFile, "C++: mrpt::maps::COccupancyGridMap3D::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("f")); cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap3D::asString() const --> std::string"); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, " @{ \n\nC++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); cl.def("assign", (class mrpt::maps::COccupancyGridMap3D & (mrpt::maps::COccupancyGridMap3D::*)(const class mrpt::maps::COccupancyGridMap3D &)) &mrpt::maps::COccupancyGridMap3D::operator=, "C++: mrpt::maps::COccupancyGridMap3D::operator=(const class mrpt::maps::COccupancyGridMap3D &) --> class mrpt::maps::COccupancyGridMap3D &", pybind11::return_value_policy::automatic, pybind11::arg("")); - { // mrpt::maps::COccupancyGridMap3D::TInsertionOptions file:mrpt/maps/COccupancyGridMap3D.h line:200 + { // mrpt::maps::COccupancyGridMap3D::TInsertionOptions file:mrpt/maps/COccupancyGridMap3D.h line:221 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_COccupancyGridMap3D_TInsertionOptions, mrpt::config::CLoadableOptions> cl(enclosing_class, "TInsertionOptions", "With this struct options are provided to the observation insertion\n process.\n \n\n CObservation::insertIntoGridMap "); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap3D::TInsertionOptions(); }, [](){ return new PyCallBack_mrpt_maps_COccupancyGridMap3D_TInsertionOptions(); } ) ); @@ -485,12 +528,13 @@ void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::s cl.def_readwrite("maxFreenessUpdateCertainty", &mrpt::maps::COccupancyGridMap3D::TInsertionOptions::maxFreenessUpdateCertainty); cl.def_readwrite("decimation_3d_range", &mrpt::maps::COccupancyGridMap3D::TInsertionOptions::decimation_3d_range); cl.def_readwrite("decimation", &mrpt::maps::COccupancyGridMap3D::TInsertionOptions::decimation); + cl.def_readwrite("raytraceEmptyCells", &mrpt::maps::COccupancyGridMap3D::TInsertionOptions::raytraceEmptyCells); cl.def("loadFromConfigFile", (void (mrpt::maps::COccupancyGridMap3D::TInsertionOptions::*)(const class mrpt::config::CConfigFileBase &, const std::string &)) &mrpt::maps::COccupancyGridMap3D::TInsertionOptions::loadFromConfigFile, "This method load the options from a \".ini\" file.\n Only those parameters found in the given \"section\" and having\n the same name that the variable are loaded. Those not found in\n the file will stay with their previous values (usually the default\n values loaded at initialization). An example of an \".ini\" file:\n \n\n\n\n\n\n \n\nC++: mrpt::maps::COccupancyGridMap3D::TInsertionOptions::loadFromConfigFile(const class mrpt::config::CConfigFileBase &, const std::string &) --> void", pybind11::arg("source"), pybind11::arg("section")); cl.def("saveToConfigFile", (void (mrpt::maps::COccupancyGridMap3D::TInsertionOptions::*)(class mrpt::config::CConfigFileBase &, const std::string &) const) &mrpt::maps::COccupancyGridMap3D::TInsertionOptions::saveToConfigFile, "C++: mrpt::maps::COccupancyGridMap3D::TInsertionOptions::saveToConfigFile(class mrpt::config::CConfigFileBase &, const std::string &) const --> void", pybind11::arg("target"), pybind11::arg("section")); cl.def("assign", (class mrpt::maps::COccupancyGridMap3D::TInsertionOptions & (mrpt::maps::COccupancyGridMap3D::TInsertionOptions::*)(const class mrpt::maps::COccupancyGridMap3D::TInsertionOptions &)) &mrpt::maps::COccupancyGridMap3D::TInsertionOptions::operator=, "C++: mrpt::maps::COccupancyGridMap3D::TInsertionOptions::operator=(const class mrpt::maps::COccupancyGridMap3D::TInsertionOptions &) --> class mrpt::maps::COccupancyGridMap3D::TInsertionOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions file:mrpt/maps/COccupancyGridMap3D.h line:257 + { // mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions file:mrpt/maps/COccupancyGridMap3D.h line:283 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_COccupancyGridMap3D_TLikelihoodOptions, mrpt::config::CLoadableOptions> cl(enclosing_class, "TLikelihoodOptions", "With this struct options are provided to the observation likelihood\n computation process "); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions(); }, [](){ return new PyCallBack_mrpt_maps_COccupancyGridMap3D_TLikelihoodOptions(); } ) ); @@ -511,10 +555,11 @@ void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::s cl.def("assign", (class mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions & (mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::*)(const class mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions &)) &mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::operator=, "C++: mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::operator=(const class mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions &) --> class mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::COccupancyGridMap3D::TRenderingOptions file:mrpt/maps/COccupancyGridMap3D.h line:311 + { // mrpt::maps::COccupancyGridMap3D::TRenderingOptions file:mrpt/maps/COccupancyGridMap3D.h line:337 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TRenderingOptions", "Options for the conversion of a mrpt::maps::COccupancyGridMap3D into a\n mrpt::opengl::COctoMapVoxels "); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap3D::TRenderingOptions(); } ) ); + cl.def( pybind11::init( [](mrpt::maps::COccupancyGridMap3D::TRenderingOptions const &o){ return new mrpt::maps::COccupancyGridMap3D::TRenderingOptions(o); } ) ); cl.def_readwrite("generateGridLines", &mrpt::maps::COccupancyGridMap3D::TRenderingOptions::generateGridLines); cl.def_readwrite("generateOccupiedVoxels", &mrpt::maps::COccupancyGridMap3D::TRenderingOptions::generateOccupiedVoxels); cl.def_readwrite("visibleOccupiedVoxels", &mrpt::maps::COccupancyGridMap3D::TRenderingOptions::visibleOccupiedVoxels); diff --git a/python/src/mrpt/maps/COctoMap.cpp b/python/src/mrpt/maps/COctoMap.cpp index f247f84d41..a6d75572a7 100644 --- a/python/src/mrpt/maps/COctoMap.cpp +++ b/python/src/mrpt/maps/COctoMap.cpp @@ -440,6 +440,19 @@ struct PyCallBack_mrpt_maps_COctoMap : public mrpt::maps::COctoMap { } return COctoMapBase::getVisualizationInto(a0); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CMetricMap::boundingBox(); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); @@ -939,6 +952,19 @@ struct PyCallBack_mrpt_maps_CSimplePointsMap : public mrpt::maps::CSimplePointsM } return CPointsMap::getVisualizationInto(a0); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CPointsMap::boundingBox(); + } double internal_computeObservationLikelihood(const class mrpt::obs::CObservation & a0, const class mrpt::poses::CPose3D & a1) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "internal_computeObservationLikelihood"); @@ -965,6 +991,32 @@ struct PyCallBack_mrpt_maps_CSimplePointsMap : public mrpt::maps::CSimplePointsM } return CPointsMap::asString(); } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2); + } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "PLY_import_set_face_count"); diff --git a/python/src/mrpt/maps/COctoMapBase.cpp b/python/src/mrpt/maps/COctoMapBase.cpp index df0f5fabe7..c08e39eb80 100644 --- a/python/src/mrpt/maps/COctoMapBase.cpp +++ b/python/src/mrpt/maps/COctoMapBase.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -164,6 +165,7 @@ void bind_mrpt_maps_COctoMapBase(std::function< pybind11::module &(std::string c cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CMetricMap::GetRuntimeClassIdStatic, "C++: mrpt::maps::CMetricMap::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); + cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::boundingBox, "Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the\n default value of mrpt::math::TBoundingBoxf() if not implemented in the\n derived class or the map is empty.\n\nC++: mrpt::maps::CMetricMap::boundingBox() const --> struct mrpt::math::TBoundingBox_"); cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); diff --git a/python/src/mrpt/maps/COctoMapBase_1.cpp b/python/src/mrpt/maps/COctoMapBase_1.cpp index 3a2ddf2b2a..265b29995f 100644 --- a/python/src/mrpt/maps/COctoMapBase_1.cpp +++ b/python/src/mrpt/maps/COctoMapBase_1.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -164,6 +165,7 @@ void bind_mrpt_maps_COctoMapBase_1(std::function< pybind11::module &(std::string cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CMetricMap::GetRuntimeClassIdStatic, "C++: mrpt::maps::CMetricMap::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); + cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::boundingBox, "Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the\n default value of mrpt::math::TBoundingBoxf() if not implemented in the\n derived class or the map is empty.\n\nC++: mrpt::maps::CMetricMap::boundingBox() const --> struct mrpt::math::TBoundingBox_"); cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); diff --git a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp index 2fa495a948..fc0d26c3b3 100644 --- a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp +++ b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp @@ -516,6 +516,19 @@ struct PyCallBack_mrpt_maps_CPointsMapXYZI : public mrpt::maps::CPointsMapXYZI { } return CPointsMap::isEmpty(); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CPointsMap::boundingBox(); + } double internal_computeObservationLikelihood(const class mrpt::obs::CObservation & a0, const class mrpt::poses::CPose3D & a1) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "internal_computeObservationLikelihood"); @@ -542,6 +555,32 @@ struct PyCallBack_mrpt_maps_CPointsMapXYZI : public mrpt::maps::CPointsMapXYZI { } return CPointsMap::asString(); } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2); + } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "PLY_import_set_face_count"); diff --git a/python/src/mrpt/maps/CPointsMap.cpp b/python/src/mrpt/maps/CPointsMap.cpp index fe105c7483..330f66a1ac 100644 --- a/python/src/mrpt/maps/CPointsMap.cpp +++ b/python/src/mrpt/maps/CPointsMap.cpp @@ -77,7 +77,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::maps::CPointsMap::TInsertionOptions file:mrpt/maps/CPointsMap.h line:226 +// mrpt::maps::CPointsMap::TInsertionOptions file:mrpt/maps/CPointsMap.h line:228 struct PyCallBack_mrpt_maps_CPointsMap_TInsertionOptions : public mrpt::maps::CPointsMap::TInsertionOptions { using mrpt::maps::CPointsMap::TInsertionOptions::TInsertionOptions; @@ -109,7 +109,7 @@ struct PyCallBack_mrpt_maps_CPointsMap_TInsertionOptions : public mrpt::maps::CP } }; -// mrpt::maps::CPointsMap::TLikelihoodOptions file:mrpt/maps/CPointsMap.h line:284 +// mrpt::maps::CPointsMap::TLikelihoodOptions file:mrpt/maps/CPointsMap.h line:286 struct PyCallBack_mrpt_maps_CPointsMap_TLikelihoodOptions : public mrpt::maps::CPointsMap::TLikelihoodOptions { using mrpt::maps::CPointsMap::TLikelihoodOptions::TLikelihoodOptions; @@ -141,7 +141,7 @@ struct PyCallBack_mrpt_maps_CPointsMap_TLikelihoodOptions : public mrpt::maps::C } }; -// mrpt::maps::CPointsMap::TRenderOptions file:mrpt/maps/CPointsMap.h line:318 +// mrpt::maps::CPointsMap::TRenderOptions file:mrpt/maps/CPointsMap.h line:320 struct PyCallBack_mrpt_maps_CPointsMap_TRenderOptions : public mrpt::maps::CPointsMap::TRenderOptions { using mrpt::maps::CPointsMap::TRenderOptions::TRenderOptions; @@ -175,8 +175,8 @@ struct PyCallBack_mrpt_maps_CPointsMap_TRenderOptions : public mrpt::maps::CPoin void bind_mrpt_maps_CPointsMap(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::CPointsMap file:mrpt/maps/CPointsMap.h line:70 - pybind11::class_, mrpt::maps::CMetricMap, mrpt::opengl::PLY_Importer, mrpt::opengl::PLY_Exporter> cl(M("mrpt::maps"), "CPointsMap", "A cloud of points in 2D or 3D, which can be built from a sequence of laser\n scans or other sensors.\n This is a virtual class, thus only a derived class can be instantiated by\n the user. The user most usually wants to use CSimplePointsMap.\n\n This class implements generic version of\n mrpt::maps::CMetric::insertObservation() accepting these types of sensory\n data:\n - mrpt::obs::CObservation2DRangeScan: 2D range scans\n - mrpt::obs::CObservation3DRangeScan: 3D range scans (Kinect, etc...)\n - mrpt::obs::CObservationRange: IRs, Sonars, etc.\n - mrpt::obs::CObservationVelodyneScan\n - mrpt::obs::CObservationPointCloud\n\n Loading and saving in the standard LAS LiDAR point cloud format is supported\n by installing `libLAS` and including the\n header `` in your program. Since MRPT 1.5.0\n there is no need to build MRPT against libLAS to use this feature.\n See LAS functions in \n\n \n CMetricMap, CPoint, CSerializable\n \n\n\n "); + { // mrpt::maps::CPointsMap file:mrpt/maps/CPointsMap.h line:71 + pybind11::class_, mrpt::maps::CMetricMap, mrpt::opengl::PLY_Importer, mrpt::opengl::PLY_Exporter, mrpt::maps::NearestNeighborsCapable> cl(M("mrpt::maps"), "CPointsMap", "A cloud of points in 2D or 3D, which can be built from a sequence of laser\n scans or other sensors.\n This is a virtual class, thus only a derived class can be instantiated by\n the user. The user most usually wants to use CSimplePointsMap.\n\n This class implements generic version of\n mrpt::maps::CMetric::insertObservation() accepting these types of sensory\n data:\n - mrpt::obs::CObservation2DRangeScan: 2D range scans\n - mrpt::obs::CObservation3DRangeScan: 3D range scans (Kinect, etc...)\n - mrpt::obs::CObservationRange: IRs, Sonars, etc.\n - mrpt::obs::CObservationVelodyneScan\n - mrpt::obs::CObservationPointCloud\n\n Loading and saving in the standard LAS LiDAR point cloud format is supported\n by installing `libLAS` and including the\n header `` in your program. Since MRPT 1.5.0\n there is no need to build MRPT against libLAS to use this feature.\n See LAS functions in \n\n \n CMetricMap, CPoint, CSerializable\n \n\n\n "); cl.def_readwrite("insertionOptions", &mrpt::maps::CPointsMap::insertionOptions); cl.def_readwrite("likelihoodOptions", &mrpt::maps::CPointsMap::likelihoodOptions); cl.def_readwrite("renderOptions", &mrpt::maps::CPointsMap::renderOptions); @@ -253,8 +253,10 @@ void bind_mrpt_maps_CPointsMap(std::function< pybind11::module &(std::string con cl.def("kdtree_distance", (float (mrpt::maps::CPointsMap::*)(const float *, size_t, size_t) const) &mrpt::maps::CPointsMap::kdtree_distance, "Returns the distance between the vector \"p1[0:size-1]\" and the data\n point with index \"idx_p2\" stored in the class:\n\nC++: mrpt::maps::CPointsMap::kdtree_distance(const float *, size_t, size_t) const --> float", pybind11::arg("p1"), pybind11::arg("idx_p2"), pybind11::arg("size")); cl.def("mark_as_modified", (void (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::mark_as_modified, "Users normally don't need to call this. Called by this class or children\n classes, set m_largestDistanceFromOriginIsUpdated=false, invalidates the\n kd-tree cache, and such. \n\nC++: mrpt::maps::CPointsMap::mark_as_modified() const --> void"); cl.def("asString", (std::string (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::CPointsMap::asString() const --> std::string"); + cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::CPointsMap::nn_single_search, " @{ \n\nC++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); - { // mrpt::maps::CPointsMap::TInsertionOptions file:mrpt/maps/CPointsMap.h line:226 + { // mrpt::maps::CPointsMap::TInsertionOptions file:mrpt/maps/CPointsMap.h line:228 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CPointsMap_TInsertionOptions, mrpt::config::CLoadableOptions> cl(enclosing_class, "TInsertionOptions", "With this struct options are provided to the observation insertion\n process.\n \n\n CObservation::insertIntoPointsMap"); cl.def( pybind11::init( [](){ return new mrpt::maps::CPointsMap::TInsertionOptions(); }, [](){ return new PyCallBack_mrpt_maps_CPointsMap_TInsertionOptions(); } ) ); @@ -275,7 +277,7 @@ void bind_mrpt_maps_CPointsMap(std::function< pybind11::module &(std::string con cl.def("assign", (struct mrpt::maps::CPointsMap::TInsertionOptions & (mrpt::maps::CPointsMap::TInsertionOptions::*)(const struct mrpt::maps::CPointsMap::TInsertionOptions &)) &mrpt::maps::CPointsMap::TInsertionOptions::operator=, "C++: mrpt::maps::CPointsMap::TInsertionOptions::operator=(const struct mrpt::maps::CPointsMap::TInsertionOptions &) --> struct mrpt::maps::CPointsMap::TInsertionOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::CPointsMap::TLikelihoodOptions file:mrpt/maps/CPointsMap.h line:284 + { // mrpt::maps::CPointsMap::TLikelihoodOptions file:mrpt/maps/CPointsMap.h line:286 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CPointsMap_TLikelihoodOptions, mrpt::config::CLoadableOptions> cl(enclosing_class, "TLikelihoodOptions", "Options used when evaluating \"computeObservationLikelihood\" in the\n derived classes.\n \n\n CObservation::computeObservationLikelihood"); cl.def( pybind11::init( [](){ return new mrpt::maps::CPointsMap::TLikelihoodOptions(); }, [](){ return new PyCallBack_mrpt_maps_CPointsMap_TLikelihoodOptions(); } ) ); @@ -290,7 +292,7 @@ void bind_mrpt_maps_CPointsMap(std::function< pybind11::module &(std::string con cl.def("assign", (struct mrpt::maps::CPointsMap::TLikelihoodOptions & (mrpt::maps::CPointsMap::TLikelihoodOptions::*)(const struct mrpt::maps::CPointsMap::TLikelihoodOptions &)) &mrpt::maps::CPointsMap::TLikelihoodOptions::operator=, "C++: mrpt::maps::CPointsMap::TLikelihoodOptions::operator=(const struct mrpt::maps::CPointsMap::TLikelihoodOptions &) --> struct mrpt::maps::CPointsMap::TLikelihoodOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::CPointsMap::TRenderOptions file:mrpt/maps/CPointsMap.h line:318 + { // mrpt::maps::CPointsMap::TRenderOptions file:mrpt/maps/CPointsMap.h line:320 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CPointsMap_TRenderOptions, mrpt::config::CLoadableOptions> cl(enclosing_class, "TRenderOptions", "Rendering options, used in getAs3DObject()"); cl.def( pybind11::init( [](){ return new mrpt::maps::CPointsMap::TRenderOptions(); }, [](){ return new PyCallBack_mrpt_maps_CPointsMap_TRenderOptions(); } ) ); diff --git a/python/src/mrpt/maps/CPointsMap_1.cpp b/python/src/mrpt/maps/CPointsMap_1.cpp index eb3a195ac2..2c7b3e91d7 100644 --- a/python/src/mrpt/maps/CPointsMap_1.cpp +++ b/python/src/mrpt/maps/CPointsMap_1.cpp @@ -39,7 +39,7 @@ void bind_mrpt_maps_CPointsMap_1(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::opengl::PointCloudAdapter file:mrpt/maps/CPointsMap.h line:1210 + { // mrpt::opengl::PointCloudAdapter file:mrpt/maps/CPointsMap.h line:1239 pybind11::class_, std::shared_ptr>> cl(M("mrpt::opengl"), "PointCloudAdapter_mrpt_maps_CPointsMap_t", "Specialization mrpt::opengl::PointCloudAdapter\n \n"); cl.def( pybind11::init(), pybind11::arg("obj") ); diff --git a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp index 77ebc71b93..1449857c70 100644 --- a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp +++ b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp @@ -273,6 +273,19 @@ struct PyCallBack_mrpt_maps_CReflectivityGridMap2D : public mrpt::maps::CReflect } return CReflectivityGridMap2D::internal_computeObservationLikelihood(a0, a1); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CMetricMap::boundingBox(); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); @@ -772,6 +785,19 @@ struct PyCallBack_mrpt_maps_CWeightedPointsMap : public mrpt::maps::CWeightedPoi } return CPointsMap::getVisualizationInto(a0); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CPointsMap::boundingBox(); + } double internal_computeObservationLikelihood(const class mrpt::obs::CObservation & a0, const class mrpt::poses::CPose3D & a1) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "internal_computeObservationLikelihood"); @@ -798,6 +824,32 @@ struct PyCallBack_mrpt_maps_CWeightedPointsMap : public mrpt::maps::CWeightedPoi } return CPointsMap::asString(); } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2); + } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "PLY_import_set_face_count"); diff --git a/python/src/mrpt/maps/CVoxelMap.cpp b/python/src/mrpt/maps/CVoxelMap.cpp index e690fdf977..90fec9256e 100644 --- a/python/src/mrpt/maps/CVoxelMap.cpp +++ b/python/src/mrpt/maps/CVoxelMap.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -90,7 +91,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::maps::CVoxelMap file:mrpt/maps/CVoxelMap.h line:33 +// mrpt::maps::CVoxelMap file:mrpt/maps/CVoxelMap.h line:38 struct PyCallBack_mrpt_maps_CVoxelMap : public mrpt::maps::CVoxelMap { using mrpt::maps::CVoxelMap::CVoxelMap; @@ -198,6 +199,45 @@ struct PyCallBack_mrpt_maps_CVoxelMap : public mrpt::maps::CVoxelMap { } return CVoxelMapOccupancyBase::getAsOctoMapVoxels(a0); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CVoxelMapOccupancyBase::boundingBox(); + } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2); + } void internal_clear() override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "internal_clear"); @@ -483,6 +523,45 @@ struct PyCallBack_mrpt_maps_CVoxelMapRGB : public mrpt::maps::CVoxelMapRGB { } return CVoxelMapOccupancyBase::getAsOctoMapVoxels(a0); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CVoxelMapOccupancyBase::boundingBox(); + } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2); + } void internal_clear() override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "internal_clear"); @@ -662,13 +741,13 @@ struct PyCallBack_mrpt_maps_CVoxelMapRGB_TMapDefinition : public mrpt::maps::CVo void bind_mrpt_maps_CVoxelMap(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::VoxelNodeOccupancy file:mrpt/maps/CVoxelMap.h line:18 + { // mrpt::maps::VoxelNodeOccupancy file:mrpt/maps/CVoxelMap.h line:23 pybind11::class_> cl(M("mrpt::maps"), "VoxelNodeOccupancy", "Voxel contents for CVoxelMap"); cl.def( pybind11::init( [](){ return new mrpt::maps::VoxelNodeOccupancy(); } ) ); cl.def_readwrite("occupancy", &mrpt::maps::VoxelNodeOccupancy::occupancy); cl.def("occupancyRef", (signed char & (mrpt::maps::VoxelNodeOccupancy::*)()) &mrpt::maps::VoxelNodeOccupancy::occupancyRef, "C++: mrpt::maps::VoxelNodeOccupancy::occupancyRef() --> signed char &", pybind11::return_value_policy::automatic); } - { // mrpt::maps::CVoxelMap file:mrpt/maps/CVoxelMap.h line:33 + { // mrpt::maps::CVoxelMap file:mrpt/maps/CVoxelMap.h line:38 pybind11::class_, PyCallBack_mrpt_maps_CVoxelMap, mrpt::maps::CVoxelMapOccupancyBase> cl(M("mrpt::maps"), "CVoxelMap", "Log-odds sparse voxel map for cells containing only the *occupancy* of each\n voxel.\n\n \n\n "); cl.def( pybind11::init( [](){ return new mrpt::maps::CVoxelMap(); }, [](){ return new PyCallBack_mrpt_maps_CVoxelMap(); } ), "doc"); cl.def( pybind11::init( [](double const & a0){ return new mrpt::maps::CVoxelMap(a0); }, [](double const & a0){ return new PyCallBack_mrpt_maps_CVoxelMap(a0); } ), "doc"); diff --git a/python/src/mrpt/maps/CVoxelMapBase.cpp b/python/src/mrpt/maps/CVoxelMapBase.cpp index ccbec05d1e..64a11658b0 100644 --- a/python/src/mrpt/maps/CVoxelMapBase.cpp +++ b/python/src/mrpt/maps/CVoxelMapBase.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include @@ -50,6 +51,7 @@ void bind_mrpt_maps_CVoxelMapBase(std::function< pybind11::module &(std::string cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CMetricMap::GetRuntimeClassIdStatic, "C++: mrpt::maps::CMetricMap::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); + cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::boundingBox, "Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the\n default value of mrpt::math::TBoundingBoxf() if not implemented in the\n derived class or the map is empty.\n\nC++: mrpt::maps::CMetricMap::boundingBox() const --> struct mrpt::math::TBoundingBox_"); cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); @@ -81,6 +83,7 @@ void bind_mrpt_maps_CVoxelMapBase(std::function< pybind11::module &(std::string cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CMetricMap::GetRuntimeClassIdStatic, "C++: mrpt::maps::CMetricMap::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); + cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::boundingBox, "Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the\n default value of mrpt::math::TBoundingBoxf() if not implemented in the\n derived class or the map is empty.\n\nC++: mrpt::maps::CMetricMap::boundingBox() const --> struct mrpt::math::TBoundingBox_"); cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase.cpp index 90de005fdd..027957aace 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase.cpp @@ -25,7 +25,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::maps::TVoxelMap_InsertionOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:22 +// mrpt::maps::TVoxelMap_InsertionOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:24 struct PyCallBack_mrpt_maps_TVoxelMap_InsertionOptions : public mrpt::maps::TVoxelMap_InsertionOptions { using mrpt::maps::TVoxelMap_InsertionOptions::TVoxelMap_InsertionOptions; @@ -57,7 +57,7 @@ struct PyCallBack_mrpt_maps_TVoxelMap_InsertionOptions : public mrpt::maps::TVox } }; -// mrpt::maps::TVoxelMap_LikelihoodOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:50 +// mrpt::maps::TVoxelMap_LikelihoodOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:52 struct PyCallBack_mrpt_maps_TVoxelMap_LikelihoodOptions : public mrpt::maps::TVoxelMap_LikelihoodOptions { using mrpt::maps::TVoxelMap_LikelihoodOptions::TVoxelMap_LikelihoodOptions; @@ -91,7 +91,7 @@ struct PyCallBack_mrpt_maps_TVoxelMap_LikelihoodOptions : public mrpt::maps::TVo void bind_mrpt_maps_CVoxelMapOccupancyBase(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::TVoxelMap_InsertionOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:22 + { // mrpt::maps::TVoxelMap_InsertionOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:24 pybind11::class_, PyCallBack_mrpt_maps_TVoxelMap_InsertionOptions, mrpt::config::CLoadableOptions> cl(M("mrpt::maps"), "TVoxelMap_InsertionOptions", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::TVoxelMap_InsertionOptions(); }, [](){ return new PyCallBack_mrpt_maps_TVoxelMap_InsertionOptions(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_maps_TVoxelMap_InsertionOptions const &o){ return new PyCallBack_mrpt_maps_TVoxelMap_InsertionOptions(o); } ) ); @@ -109,7 +109,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase(std::function< pybind11::module &(std cl.def("readFromStream", (void (mrpt::maps::TVoxelMap_InsertionOptions::*)(class mrpt::serialization::CArchive &)) &mrpt::maps::TVoxelMap_InsertionOptions::readFromStream, "C++: mrpt::maps::TVoxelMap_InsertionOptions::readFromStream(class mrpt::serialization::CArchive &) --> void", pybind11::arg("in")); cl.def("assign", (struct mrpt::maps::TVoxelMap_InsertionOptions & (mrpt::maps::TVoxelMap_InsertionOptions::*)(const struct mrpt::maps::TVoxelMap_InsertionOptions &)) &mrpt::maps::TVoxelMap_InsertionOptions::operator=, "C++: mrpt::maps::TVoxelMap_InsertionOptions::operator=(const struct mrpt::maps::TVoxelMap_InsertionOptions &) --> struct mrpt::maps::TVoxelMap_InsertionOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::TVoxelMap_LikelihoodOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:50 + { // mrpt::maps::TVoxelMap_LikelihoodOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:52 pybind11::class_, PyCallBack_mrpt_maps_TVoxelMap_LikelihoodOptions, mrpt::config::CLoadableOptions> cl(M("mrpt::maps"), "TVoxelMap_LikelihoodOptions", "Options used when evaluating \"computeObservationLikelihood\"\n \n\n CObservation::computeObservationLikelihood"); cl.def( pybind11::init( [](){ return new mrpt::maps::TVoxelMap_LikelihoodOptions(); }, [](){ return new PyCallBack_mrpt_maps_TVoxelMap_LikelihoodOptions(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_maps_TVoxelMap_LikelihoodOptions const &o){ return new PyCallBack_mrpt_maps_TVoxelMap_LikelihoodOptions(o); } ) ); @@ -122,7 +122,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase(std::function< pybind11::module &(std cl.def("readFromStream", (void (mrpt::maps::TVoxelMap_LikelihoodOptions::*)(class mrpt::serialization::CArchive &)) &mrpt::maps::TVoxelMap_LikelihoodOptions::readFromStream, "C++: mrpt::maps::TVoxelMap_LikelihoodOptions::readFromStream(class mrpt::serialization::CArchive &) --> void", pybind11::arg("in")); cl.def("assign", (struct mrpt::maps::TVoxelMap_LikelihoodOptions & (mrpt::maps::TVoxelMap_LikelihoodOptions::*)(const struct mrpt::maps::TVoxelMap_LikelihoodOptions &)) &mrpt::maps::TVoxelMap_LikelihoodOptions::operator=, "C++: mrpt::maps::TVoxelMap_LikelihoodOptions::operator=(const struct mrpt::maps::TVoxelMap_LikelihoodOptions &) --> struct mrpt::maps::TVoxelMap_LikelihoodOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::TVoxelMap_RenderingOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:75 + { // mrpt::maps::TVoxelMap_RenderingOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:77 pybind11::class_> cl(M("mrpt::maps"), "TVoxelMap_RenderingOptions", "Options for the conversion of a mrpt::maps::COctoMap into a\n mrpt::opengl::COctoMapVoxels "); cl.def( pybind11::init( [](){ return new mrpt::maps::TVoxelMap_RenderingOptions(); } ) ); cl.def( pybind11::init( [](mrpt::maps::TVoxelMap_RenderingOptions const &o){ return new mrpt::maps::TVoxelMap_RenderingOptions(o); } ) ); diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp index 6798ce7e7f..45a1bd2307 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -49,8 +50,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::CVoxelMapOccupancyBase file:mrpt/maps/CVoxelMapOccupancyBase.h line:114 - pybind11::class_, std::shared_ptr>, mrpt::maps::CVoxelMapBase> cl(M("mrpt::maps"), "CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t", ""); + { // mrpt::maps::CVoxelMapOccupancyBase file:mrpt/maps/CVoxelMapOccupancyBase.h line:116 + pybind11::class_, std::shared_ptr>, mrpt::maps::CVoxelMapBase, mrpt::maps::NearestNeighborsCapable> cl(M("mrpt::maps"), "CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t", ""); cl.def_readwrite("insertionOptions", &mrpt::maps::CVoxelMapOccupancyBase::insertionOptions); cl.def_readwrite("likelihoodOptions", &mrpt::maps::CVoxelMapOccupancyBase::likelihoodOptions); cl.def_readwrite("renderingOptions", &mrpt::maps::CVoxelMapOccupancyBase::renderingOptions); @@ -58,15 +59,16 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s cl.def("getAsOctoMapVoxels", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(class mrpt::opengl::COctoMapVoxels &) const) &mrpt::maps::CVoxelMapOccupancyBase::getAsOctoMapVoxels, "C++: mrpt::maps::CVoxelMapOccupancyBase::getAsOctoMapVoxels(class mrpt::opengl::COctoMapVoxels &) const --> void", pybind11::arg("gl_obj")); cl.def("updateVoxel", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(const double, const double, const double, bool)) &mrpt::maps::CVoxelMapOccupancyBase::updateVoxel, "C++: mrpt::maps::CVoxelMapOccupancyBase::updateVoxel(const double, const double, const double, bool) --> void", pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("occupied")); cl.def("getPointOccupancy", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const double, const double, const double, double &) const) &mrpt::maps::CVoxelMapOccupancyBase::getPointOccupancy, "C++: mrpt::maps::CVoxelMapOccupancyBase::getPointOccupancy(const double, const double, const double, double &) const --> bool", pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("prob_occupancy")); - cl.def("insertPointCloudAsRays", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &)) &mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsRays, "C++: mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsRays(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &) --> void", pybind11::arg("pts"), pybind11::arg("sensorPt")); - cl.def("insertPointCloudAsEndPoints", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &)) &mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsEndPoints, "C++: mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsEndPoints(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &) --> void", pybind11::arg("pts"), pybind11::arg("sensorPt")); cl.def("getOccupiedVoxels", (class std::shared_ptr (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::getOccupiedVoxels, "C++: mrpt::maps::CVoxelMapOccupancyBase::getOccupiedVoxels() const --> class std::shared_ptr"); + cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::boundingBox, "C++: mrpt::maps::CVoxelMapOccupancyBase::boundingBox() const --> struct mrpt::math::TBoundingBox_"); cl.def("updateCell_fast_occupied", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(struct mrpt::maps::VoxelNodeOccupancy *, const signed char, const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_occupied, "C++: mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_occupied(struct mrpt::maps::VoxelNodeOccupancy *, const signed char, const signed char) --> void", pybind11::arg("theCell"), pybind11::arg("logodd_obs"), pybind11::arg("thres")); cl.def("updateCell_fast_free", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(struct mrpt::maps::VoxelNodeOccupancy *, const signed char, const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_free, "C++: mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_free(struct mrpt::maps::VoxelNodeOccupancy *, const signed char, const signed char) --> void", pybind11::arg("theCell"), pybind11::arg("logodd_obs"), pybind11::arg("thres")); cl.def_static("get_logodd_lut", (struct mrpt::maps::CLogOddsGridMapLUT & (*)()) &mrpt::maps::CVoxelMapOccupancyBase::get_logodd_lut, "C++: mrpt::maps::CVoxelMapOccupancyBase::get_logodd_lut() --> struct mrpt::maps::CLogOddsGridMapLUT &", pybind11::return_value_policy::automatic); cl.def_static("l2p", (float (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p(const signed char) --> float", pybind11::arg("l")); cl.def_static("l2p_255", (uint8_t (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p_255, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p_255(const signed char) --> uint8_t", pybind11::arg("l")); cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); @@ -78,6 +80,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CMetricMap::GetRuntimeClassIdStatic, "C++: mrpt::maps::CMetricMap::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); + cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::boundingBox, "Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the\n default value of mrpt::math::TBoundingBoxf() if not implemented in the\n derived class or the map is empty.\n\nC++: mrpt::maps::CMetricMap::boundingBox() const --> struct mrpt::math::TBoundingBox_"); cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); @@ -96,5 +99,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s cl.def("squareDistanceToClosestCorrespondence", (float (mrpt::maps::CMetricMap::*)(float, float) const) &mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence, "Returns the square distance from the 2D point (x0,y0) to the closest\n correspondence in the map. \n\nC++: mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence(float, float) const --> float", pybind11::arg("x0"), pybind11::arg("y0")); cl.def("getAsSimplePointsMap", (class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMetricMap::getAsSimplePointsMap() --> class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp index 2cb5a1f559..6deee581f7 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -49,8 +50,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::CVoxelMapOccupancyBase file:mrpt/maps/CVoxelMapOccupancyBase.h line:114 - pybind11::class_, std::shared_ptr>, mrpt::maps::CVoxelMapBase> cl(M("mrpt::maps"), "CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t", ""); + { // mrpt::maps::CVoxelMapOccupancyBase file:mrpt/maps/CVoxelMapOccupancyBase.h line:116 + pybind11::class_, std::shared_ptr>, mrpt::maps::CVoxelMapBase, mrpt::maps::NearestNeighborsCapable> cl(M("mrpt::maps"), "CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t", ""); cl.def_readwrite("insertionOptions", &mrpt::maps::CVoxelMapOccupancyBase::insertionOptions); cl.def_readwrite("likelihoodOptions", &mrpt::maps::CVoxelMapOccupancyBase::likelihoodOptions); cl.def_readwrite("renderingOptions", &mrpt::maps::CVoxelMapOccupancyBase::renderingOptions); @@ -58,15 +59,16 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s cl.def("getAsOctoMapVoxels", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(class mrpt::opengl::COctoMapVoxels &) const) &mrpt::maps::CVoxelMapOccupancyBase::getAsOctoMapVoxels, "C++: mrpt::maps::CVoxelMapOccupancyBase::getAsOctoMapVoxels(class mrpt::opengl::COctoMapVoxels &) const --> void", pybind11::arg("gl_obj")); cl.def("updateVoxel", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(const double, const double, const double, bool)) &mrpt::maps::CVoxelMapOccupancyBase::updateVoxel, "C++: mrpt::maps::CVoxelMapOccupancyBase::updateVoxel(const double, const double, const double, bool) --> void", pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("occupied")); cl.def("getPointOccupancy", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const double, const double, const double, double &) const) &mrpt::maps::CVoxelMapOccupancyBase::getPointOccupancy, "C++: mrpt::maps::CVoxelMapOccupancyBase::getPointOccupancy(const double, const double, const double, double &) const --> bool", pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("prob_occupancy")); - cl.def("insertPointCloudAsRays", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &)) &mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsRays, "C++: mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsRays(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &) --> void", pybind11::arg("pts"), pybind11::arg("sensorPt")); - cl.def("insertPointCloudAsEndPoints", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &)) &mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsEndPoints, "C++: mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsEndPoints(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &) --> void", pybind11::arg("pts"), pybind11::arg("sensorPt")); cl.def("getOccupiedVoxels", (class std::shared_ptr (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::getOccupiedVoxels, "C++: mrpt::maps::CVoxelMapOccupancyBase::getOccupiedVoxels() const --> class std::shared_ptr"); + cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::boundingBox, "C++: mrpt::maps::CVoxelMapOccupancyBase::boundingBox() const --> struct mrpt::math::TBoundingBox_"); cl.def("updateCell_fast_occupied", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(struct mrpt::maps::VoxelNodeOccRGB *, const signed char, const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_occupied, "C++: mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_occupied(struct mrpt::maps::VoxelNodeOccRGB *, const signed char, const signed char) --> void", pybind11::arg("theCell"), pybind11::arg("logodd_obs"), pybind11::arg("thres")); cl.def("updateCell_fast_free", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(struct mrpt::maps::VoxelNodeOccRGB *, const signed char, const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_free, "C++: mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_free(struct mrpt::maps::VoxelNodeOccRGB *, const signed char, const signed char) --> void", pybind11::arg("theCell"), pybind11::arg("logodd_obs"), pybind11::arg("thres")); cl.def_static("get_logodd_lut", (struct mrpt::maps::CLogOddsGridMapLUT & (*)()) &mrpt::maps::CVoxelMapOccupancyBase::get_logodd_lut, "C++: mrpt::maps::CVoxelMapOccupancyBase::get_logodd_lut() --> struct mrpt::maps::CLogOddsGridMapLUT &", pybind11::return_value_policy::automatic); cl.def_static("l2p", (float (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p(const signed char) --> float", pybind11::arg("l")); cl.def_static("l2p_255", (uint8_t (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p_255, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p_255(const signed char) --> uint8_t", pybind11::arg("l")); cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); @@ -78,6 +80,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CMetricMap::GetRuntimeClassIdStatic, "C++: mrpt::maps::CMetricMap::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); + cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::boundingBox, "Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the\n default value of mrpt::math::TBoundingBoxf() if not implemented in the\n derived class or the map is empty.\n\nC++: mrpt::maps::CMetricMap::boundingBox() const --> struct mrpt::math::TBoundingBox_"); cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); @@ -96,5 +99,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s cl.def("squareDistanceToClosestCorrespondence", (float (mrpt::maps::CMetricMap::*)(float, float) const) &mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence, "Returns the square distance from the 2D point (x0,y0) to the closest\n correspondence in the map. \n\nC++: mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence(float, float) const --> float", pybind11::arg("x0"), pybind11::arg("y0")); cl.def("getAsSimplePointsMap", (class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMetricMap::getAsSimplePointsMap() --> class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/maps/CWirelessPowerGridMap2D.cpp b/python/src/mrpt/maps/CWirelessPowerGridMap2D.cpp index 6a84a1043a..05c473597e 100644 --- a/python/src/mrpt/maps/CWirelessPowerGridMap2D.cpp +++ b/python/src/mrpt/maps/CWirelessPowerGridMap2D.cpp @@ -383,6 +383,19 @@ struct PyCallBack_mrpt_maps_CWirelessPowerGridMap2D : public mrpt::maps::CWirele } return CRandomFieldGridMap2D::predictMeasurement(a0, a1, a2, a3, a4, a5); } + struct mrpt::math::TBoundingBox_ boundingBox() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "boundingBox"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference>::value) { + static pybind11::detail::override_caster_t> caster; + return pybind11::detail::cast_ref>(std::move(o), caster); + } + else return pybind11::detail::cast_safe>(std::move(o)); + } + return CMetricMap::boundingBox(); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); diff --git a/python/src/mrpt/maps/NearestNeighborsCapable.cpp b/python/src/mrpt/maps/NearestNeighborsCapable.cpp new file mode 100644 index 0000000000..c62314f21c --- /dev/null +++ b/python/src/mrpt/maps/NearestNeighborsCapable.cpp @@ -0,0 +1,33 @@ +#include +#include +#include +#include +#include +#include +#include +#include // __str__ +#include +#include + +#include +#include +#include +#include + + +#ifndef BINDER_PYBIND11_TYPE_CASTER + #define BINDER_PYBIND11_TYPE_CASTER + PYBIND11_DECLARE_HOLDER_TYPE(T, std::shared_ptr) + PYBIND11_DECLARE_HOLDER_TYPE(T, T*) + PYBIND11_MAKE_OPAQUE(std::shared_ptr) +#endif + +void bind_mrpt_maps_NearestNeighborsCapable(std::function< pybind11::module &(std::string const &namespace_) > &M) +{ + { // mrpt::maps::NearestNeighborsCapable file:mrpt/maps/NearestNeighborsCapable.h line:26 + pybind11::class_> cl(M("mrpt::maps"), "NearestNeighborsCapable", "Virtual interface for maps having the capability of searching the closest\n neighbor(s) of a given query 2D or 3D point.\n\n Note this is more generic than mrpt::math::KDTreeCapable since it does not\n assume the use of KD-trees, and it is also non templatized, so users can use\n dynamic casting to interact with maps in a generic way.\n\n \n New in MRPT 2.11.3\n \n\n\n "); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); + } +} diff --git a/python/src/mrpt/math/TBoundingBox.cpp b/python/src/mrpt/math/TBoundingBox.cpp index 9534b51f2d..729f67bebb 100644 --- a/python/src/mrpt/math/TBoundingBox.cpp +++ b/python/src/mrpt/math/TBoundingBox.cpp @@ -57,6 +57,7 @@ void bind_mrpt_math_TBoundingBox(std::function< pybind11::module &(std::string c cl.def("updateWithPoint", (void (mrpt::math::TBoundingBox_::*)(const struct mrpt::math::TPoint3D_ &)) &mrpt::math::TBoundingBox_::updateWithPoint, "C++: mrpt::math::TBoundingBox_::updateWithPoint(const struct mrpt::math::TPoint3D_ &) --> void", pybind11::arg("p")); cl.def("containsPoint", (bool (mrpt::math::TBoundingBox_::*)(const struct mrpt::math::TPoint3D_ &) const) &mrpt::math::TBoundingBox_::containsPoint, "C++: mrpt::math::TBoundingBox_::containsPoint(const struct mrpt::math::TPoint3D_ &) const --> bool", pybind11::arg("p")); cl.def("asString", (std::string (mrpt::math::TBoundingBox_::*)() const) &mrpt::math::TBoundingBox_::asString, "C++: mrpt::math::TBoundingBox_::asString() const --> std::string"); + cl.def("__eq__", (bool (mrpt::math::TBoundingBox_::*)(const struct mrpt::math::TBoundingBox_ &) const) &mrpt::math::TBoundingBox_::operator==, "C++: mrpt::math::TBoundingBox_::operator==(const struct mrpt::math::TBoundingBox_ &) const --> bool", pybind11::arg("o")); cl.def("assign", (struct mrpt::math::TBoundingBox_ & (mrpt::math::TBoundingBox_::*)(const struct mrpt::math::TBoundingBox_ &)) &mrpt::math::TBoundingBox_::operator=, "C++: mrpt::math::TBoundingBox_::operator=(const struct mrpt::math::TBoundingBox_ &) --> struct mrpt::math::TBoundingBox_ &", pybind11::return_value_policy::automatic, pybind11::arg("")); } { // mrpt::math::TBoundingBox_ file:mrpt/math/TBoundingBox.h line:27 @@ -80,6 +81,7 @@ void bind_mrpt_math_TBoundingBox(std::function< pybind11::module &(std::string c cl.def("updateWithPoint", (void (mrpt::math::TBoundingBox_::*)(const struct mrpt::math::TPoint3D_ &)) &mrpt::math::TBoundingBox_::updateWithPoint, "C++: mrpt::math::TBoundingBox_::updateWithPoint(const struct mrpt::math::TPoint3D_ &) --> void", pybind11::arg("p")); cl.def("containsPoint", (bool (mrpt::math::TBoundingBox_::*)(const struct mrpt::math::TPoint3D_ &) const) &mrpt::math::TBoundingBox_::containsPoint, "C++: mrpt::math::TBoundingBox_::containsPoint(const struct mrpt::math::TPoint3D_ &) const --> bool", pybind11::arg("p")); cl.def("asString", (std::string (mrpt::math::TBoundingBox_::*)() const) &mrpt::math::TBoundingBox_::asString, "C++: mrpt::math::TBoundingBox_::asString() const --> std::string"); + cl.def("__eq__", (bool (mrpt::math::TBoundingBox_::*)(const struct mrpt::math::TBoundingBox_ &) const) &mrpt::math::TBoundingBox_::operator==, "C++: mrpt::math::TBoundingBox_::operator==(const struct mrpt::math::TBoundingBox_ &) const --> bool", pybind11::arg("o")); cl.def("assign", (struct mrpt::math::TBoundingBox_ & (mrpt::math::TBoundingBox_::*)(const struct mrpt::math::TBoundingBox_ &)) &mrpt::math::TBoundingBox_::operator=, "C++: mrpt::math::TBoundingBox_::operator=(const struct mrpt::math::TBoundingBox_ &) --> struct mrpt::math::TBoundingBox_ &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/math/TPoint2D.cpp b/python/src/mrpt/math/TPoint2D.cpp index d137dc43c2..65d5fc0947 100644 --- a/python/src/mrpt/math/TPoint2D.cpp +++ b/python/src/mrpt/math/TPoint2D.cpp @@ -25,7 +25,7 @@ void bind_mrpt_math_TPoint2D(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::math::TPoint2D_ file:mrpt/math/TPoint2D.h line:35 + { // mrpt::math::TPoint2D_ file:mrpt/math/TPoint2D.h line:38 pybind11::class_, std::shared_ptr>, mrpt::math::TPoseOrPoint, mrpt::math::TPoint2D_data> cl(M("mrpt::math"), "TPoint2D_double_t", ""); cl.def( pybind11::init( [](){ return new mrpt::math::TPoint2D_(); } ) ); cl.def( pybind11::init(), pybind11::arg("xx"), pybind11::arg("yy") ); @@ -59,7 +59,7 @@ void bind_mrpt_math_TPoint2D(std::function< pybind11::module &(std::string const cl.def_readwrite("y", &mrpt::math::TPoint2D_data::y); cl.def("assign", (struct mrpt::math::TPoint2D_data & (mrpt::math::TPoint2D_data::*)(const struct mrpt::math::TPoint2D_data &)) &mrpt::math::TPoint2D_data::operator=, "C++: mrpt::math::TPoint2D_data::operator=(const struct mrpt::math::TPoint2D_data &) --> struct mrpt::math::TPoint2D_data &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::math::TPoint2D_ file:mrpt/math/TPoint2D.h line:35 + { // mrpt::math::TPoint2D_ file:mrpt/math/TPoint2D.h line:38 pybind11::class_, std::shared_ptr>, mrpt::math::TPoseOrPoint, mrpt::math::TPoint2D_data> cl(M("mrpt::math"), "TPoint2D_float_t", ""); cl.def( pybind11::init( [](){ return new mrpt::math::TPoint2D_(); } ) ); cl.def( pybind11::init(), pybind11::arg("xx"), pybind11::arg("yy") ); @@ -93,7 +93,7 @@ void bind_mrpt_math_TPoint2D(std::function< pybind11::module &(std::string const cl.def_readwrite("y", &mrpt::math::TPoint2D_data::y); cl.def("assign", (struct mrpt::math::TPoint2D_data & (mrpt::math::TPoint2D_data::*)(const struct mrpt::math::TPoint2D_data &)) &mrpt::math::TPoint2D_data::operator=, "C++: mrpt::math::TPoint2D_data::operator=(const struct mrpt::math::TPoint2D_data &) --> struct mrpt::math::TPoint2D_data &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::math::TPoint3D_ file:mrpt/math/TPoint3D.h line:42 + { // mrpt::math::TPoint3D_ file:mrpt/math/TPoint3D.h line:45 pybind11::class_, std::shared_ptr>, mrpt::math::TPoseOrPoint, mrpt::math::TPoint3D_data> cl(M("mrpt::math"), "TPoint3D_double_t", ""); cl.def( pybind11::init( [](){ return new mrpt::math::TPoint3D_(); } ) ); cl.def( pybind11::init(), pybind11::arg("xx"), pybind11::arg("yy"), pybind11::arg("zz") ); diff --git a/python/src/mrpt/math/TPoint3D.cpp b/python/src/mrpt/math/TPoint3D.cpp index f7b39eb71e..a213e21eb2 100644 --- a/python/src/mrpt/math/TPoint3D.cpp +++ b/python/src/mrpt/math/TPoint3D.cpp @@ -18,8 +18,10 @@ void bind_mrpt_math_TPoint3D(std::function< pybind11::module &(std::string const { { // mrpt::math::TPoint3D_data file:mrpt/math/TPoint3D.h line:31 pybind11::class_, std::shared_ptr>> cl(M("mrpt::math"), "TPoint3D_data_double_t", ""); - cl.def( pybind11::init( [](mrpt::math::TPoint3D_data const &o){ return new mrpt::math::TPoint3D_data(o); } ) ); cl.def( pybind11::init( [](){ return new mrpt::math::TPoint3D_data(); } ) ); + cl.def( pybind11::init(), pybind11::arg("X"), pybind11::arg("Y"), pybind11::arg("Z") ); + + cl.def( pybind11::init( [](mrpt::math::TPoint3D_data const &o){ return new mrpt::math::TPoint3D_data(o); } ) ); cl.def_readwrite("x", &mrpt::math::TPoint3D_data::x); cl.def_readwrite("y", &mrpt::math::TPoint3D_data::y); cl.def_readwrite("z", &mrpt::math::TPoint3D_data::z); @@ -27,14 +29,16 @@ void bind_mrpt_math_TPoint3D(std::function< pybind11::module &(std::string const } { // mrpt::math::TPoint3D_data file:mrpt/math/TPoint3D.h line:31 pybind11::class_, std::shared_ptr>> cl(M("mrpt::math"), "TPoint3D_data_float_t", ""); - cl.def( pybind11::init( [](mrpt::math::TPoint3D_data const &o){ return new mrpt::math::TPoint3D_data(o); } ) ); cl.def( pybind11::init( [](){ return new mrpt::math::TPoint3D_data(); } ) ); + cl.def( pybind11::init(), pybind11::arg("X"), pybind11::arg("Y"), pybind11::arg("Z") ); + + cl.def( pybind11::init( [](mrpt::math::TPoint3D_data const &o){ return new mrpt::math::TPoint3D_data(o); } ) ); cl.def_readwrite("x", &mrpt::math::TPoint3D_data::x); cl.def_readwrite("y", &mrpt::math::TPoint3D_data::y); cl.def_readwrite("z", &mrpt::math::TPoint3D_data::z); cl.def("assign", (struct mrpt::math::TPoint3D_data & (mrpt::math::TPoint3D_data::*)(const struct mrpt::math::TPoint3D_data &)) &mrpt::math::TPoint3D_data::operator=, "C++: mrpt::math::TPoint3D_data::operator=(const struct mrpt::math::TPoint3D_data &) --> struct mrpt::math::TPoint3D_data &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::math::TPointXYZIu8 file:mrpt/math/TPoint3D.h line:301 + { // mrpt::math::TPointXYZIu8 file:mrpt/math/TPoint3D.h line:304 pybind11::class_> cl(M("mrpt::math"), "TPointXYZIu8", "XYZ point (double) + Intensity(u8) \n mrpt::math::TPoint3D "); cl.def( pybind11::init( [](){ return new mrpt::math::TPointXYZIu8(); } ) ); cl.def( pybind11::init(), pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("intensity_val") ); @@ -42,7 +46,7 @@ void bind_mrpt_math_TPoint3D(std::function< pybind11::module &(std::string const cl.def_readwrite("pt", &mrpt::math::TPointXYZIu8::pt); cl.def_readwrite("intensity", &mrpt::math::TPointXYZIu8::intensity); } - { // mrpt::math::TPointXYZRGBu8 file:mrpt/math/TPoint3D.h line:312 + { // mrpt::math::TPointXYZRGBu8 file:mrpt/math/TPoint3D.h line:315 pybind11::class_> cl(M("mrpt::math"), "TPointXYZRGBu8", "XYZ point (double) + RGB(u8) \n mrpt::math::TPoint3D "); cl.def( pybind11::init( [](){ return new mrpt::math::TPointXYZRGBu8(); } ) ); cl.def( pybind11::init(), pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("R_val"), pybind11::arg("G_val"), pybind11::arg("B_val") ); @@ -52,7 +56,7 @@ void bind_mrpt_math_TPoint3D(std::function< pybind11::module &(std::string const cl.def_readwrite("g", &mrpt::math::TPointXYZRGBu8::g); cl.def_readwrite("b", &mrpt::math::TPointXYZRGBu8::b); } - { // mrpt::math::TPointXYZfIu8 file:mrpt/math/TPoint3D.h line:325 + { // mrpt::math::TPointXYZfIu8 file:mrpt/math/TPoint3D.h line:328 pybind11::class_> cl(M("mrpt::math"), "TPointXYZfIu8", "XYZ point (float) + Intensity(u8) \n mrpt::math::TPoint3D "); cl.def( pybind11::init( [](){ return new mrpt::math::TPointXYZfIu8(); } ) ); cl.def( pybind11::init(), pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("intensity_val") ); @@ -60,7 +64,7 @@ void bind_mrpt_math_TPoint3D(std::function< pybind11::module &(std::string const cl.def_readwrite("pt", &mrpt::math::TPointXYZfIu8::pt); cl.def_readwrite("intensity", &mrpt::math::TPointXYZfIu8::intensity); } - { // mrpt::math::TPointXYZfRGBu8 file:mrpt/math/TPoint3D.h line:336 + { // mrpt::math::TPointXYZfRGBu8 file:mrpt/math/TPoint3D.h line:339 pybind11::class_> cl(M("mrpt::math"), "TPointXYZfRGBu8", "XYZ point (float) + RGB(u8) \n mrpt::math::TPoint3D "); cl.def( pybind11::init( [](){ return new mrpt::math::TPointXYZfRGBu8(); } ) ); cl.def( pybind11::init(), pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("R_val"), pybind11::arg("G_val"), pybind11::arg("B_val") ); @@ -70,7 +74,7 @@ void bind_mrpt_math_TPoint3D(std::function< pybind11::module &(std::string const cl.def_readwrite("g", &mrpt::math::TPointXYZfRGBu8::g); cl.def_readwrite("b", &mrpt::math::TPointXYZfRGBu8::b); } - { // mrpt::math::TPointXYZfRGBAu8 file:mrpt/math/TPoint3D.h line:354 + { // mrpt::math::TPointXYZfRGBAu8 file:mrpt/math/TPoint3D.h line:357 pybind11::class_> cl(M("mrpt::math"), "TPointXYZfRGBAu8", "XYZ point (float) + RGBA(u8) \n mrpt::math::TPoint3D "); cl.def( pybind11::init( [](){ return new mrpt::math::TPointXYZfRGBAu8(); } ) ); cl.def( pybind11::init( [](float const & a0, float const & a1, float const & a2, uint8_t const & a3, uint8_t const & a4, uint8_t const & a5){ return new mrpt::math::TPointXYZfRGBAu8(a0, a1, a2, a3, a4, a5); } ), "doc" , pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("R_val"), pybind11::arg("G_val"), pybind11::arg("B_val")); @@ -84,7 +88,7 @@ void bind_mrpt_math_TPoint3D(std::function< pybind11::module &(std::string const cl.def_readwrite("a", &mrpt::math::TPointXYZfRGBAu8::a); cl.def("assign", (struct mrpt::math::TPointXYZfRGBAu8 & (mrpt::math::TPointXYZfRGBAu8::*)(const struct mrpt::math::TPointXYZfRGBAu8 &)) &mrpt::math::TPointXYZfRGBAu8::operator=, "C++: mrpt::math::TPointXYZfRGBAu8::operator=(const struct mrpt::math::TPointXYZfRGBAu8 &) --> struct mrpt::math::TPointXYZfRGBAu8 &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::math::TPointXYZRGBAf file:mrpt/math/TPoint3D.h line:374 + { // mrpt::math::TPointXYZRGBAf file:mrpt/math/TPoint3D.h line:377 pybind11::class_> cl(M("mrpt::math"), "TPointXYZRGBAf", "XYZ point (float) + RGBA(float) [1-byte memory packed, no padding]\n \n\n mrpt::math::TPoint3D "); cl.def( pybind11::init( [](){ return new mrpt::math::TPointXYZRGBAf(); } ) ); cl.def( pybind11::init(), pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("R_val"), pybind11::arg("G_val"), pybind11::arg("B_val"), pybind11::arg("A_val") ); diff --git a/python/src/mrpt/math/TPoint3D_1.cpp b/python/src/mrpt/math/TPoint3D_1.cpp index d1349ee973..fd14cccaf9 100644 --- a/python/src/mrpt/math/TPoint3D_1.cpp +++ b/python/src/mrpt/math/TPoint3D_1.cpp @@ -27,7 +27,7 @@ void bind_mrpt_math_TPoint3D_1(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::math::TPoint3D_ file:mrpt/math/TPoint3D.h line:42 + { // mrpt::math::TPoint3D_ file:mrpt/math/TPoint3D.h line:45 pybind11::class_, std::shared_ptr>, mrpt::math::TPoseOrPoint, mrpt::math::TPoint3D_data> cl(M("mrpt::math"), "TPoint3D_float_t", ""); cl.def( pybind11::init( [](){ return new mrpt::math::TPoint3D_(); } ) ); cl.def( pybind11::init(), pybind11::arg("xx"), pybind11::arg("yy"), pybind11::arg("zz") ); diff --git a/python/src/mrpt/math/TPoseOrPoint.cpp b/python/src/mrpt/math/TPoseOrPoint.cpp index 5d259586b5..b7c9d1ba0d 100644 --- a/python/src/mrpt/math/TPoseOrPoint.cpp +++ b/python/src/mrpt/math/TPoseOrPoint.cpp @@ -25,16 +25,20 @@ void bind_mrpt_math_TPoseOrPoint(std::function< pybind11::module &(std::string c } { // mrpt::math::TPoint2D_data file:mrpt/math/TPoint2D.h line:26 pybind11::class_, std::shared_ptr>> cl(M("mrpt::math"), "TPoint2D_data_double_t", ""); - cl.def( pybind11::init( [](mrpt::math::TPoint2D_data const &o){ return new mrpt::math::TPoint2D_data(o); } ) ); cl.def( pybind11::init( [](){ return new mrpt::math::TPoint2D_data(); } ) ); + cl.def( pybind11::init(), pybind11::arg("X"), pybind11::arg("Y") ); + + cl.def( pybind11::init( [](mrpt::math::TPoint2D_data const &o){ return new mrpt::math::TPoint2D_data(o); } ) ); cl.def_readwrite("x", &mrpt::math::TPoint2D_data::x); cl.def_readwrite("y", &mrpt::math::TPoint2D_data::y); cl.def("assign", (struct mrpt::math::TPoint2D_data & (mrpt::math::TPoint2D_data::*)(const struct mrpt::math::TPoint2D_data &)) &mrpt::math::TPoint2D_data::operator=, "C++: mrpt::math::TPoint2D_data::operator=(const struct mrpt::math::TPoint2D_data &) --> struct mrpt::math::TPoint2D_data &", pybind11::return_value_policy::automatic, pybind11::arg("")); } { // mrpt::math::TPoint2D_data file:mrpt/math/TPoint2D.h line:26 pybind11::class_, std::shared_ptr>> cl(M("mrpt::math"), "TPoint2D_data_float_t", ""); - cl.def( pybind11::init( [](mrpt::math::TPoint2D_data const &o){ return new mrpt::math::TPoint2D_data(o); } ) ); cl.def( pybind11::init( [](){ return new mrpt::math::TPoint2D_data(); } ) ); + cl.def( pybind11::init(), pybind11::arg("X"), pybind11::arg("Y") ); + + cl.def( pybind11::init( [](mrpt::math::TPoint2D_data const &o){ return new mrpt::math::TPoint2D_data(o); } ) ); cl.def_readwrite("x", &mrpt::math::TPoint2D_data::x); cl.def_readwrite("y", &mrpt::math::TPoint2D_data::y); cl.def("assign", (struct mrpt::math::TPoint2D_data & (mrpt::math::TPoint2D_data::*)(const struct mrpt::math::TPoint2D_data &)) &mrpt::math::TPoint2D_data::operator=, "C++: mrpt::math::TPoint2D_data::operator=(const struct mrpt::math::TPoint2D_data &) --> struct mrpt::math::TPoint2D_data &", pybind11::return_value_policy::automatic, pybind11::arg("")); diff --git a/python/src/mrpt/obs/CObservationRange.cpp b/python/src/mrpt/obs/CObservationRange.cpp index 9d3399bd33..a34de46c7e 100644 --- a/python/src/mrpt/obs/CObservationRange.cpp +++ b/python/src/mrpt/obs/CObservationRange.cpp @@ -263,6 +263,7 @@ void bind_mrpt_obs_CObservationRange(std::function< pybind11::module &(std::stri cl.def_readwrite("sensorID", &mrpt::obs::CObservationRange::TMeasurement::sensorID); cl.def_readwrite("sensorPose", &mrpt::obs::CObservationRange::TMeasurement::sensorPose); cl.def_readwrite("sensedDistance", &mrpt::obs::CObservationRange::TMeasurement::sensedDistance); + cl.def_readwrite("sensorNoiseStdDeviation", &mrpt::obs::CObservationRange::TMeasurement::sensorNoiseStdDeviation); } } diff --git a/python/src/mrpt/obs/CSensoryFrame.cpp b/python/src/mrpt/obs/CSensoryFrame.cpp index 7822938b41..ce87bdeb6a 100644 --- a/python/src/mrpt/obs/CSensoryFrame.cpp +++ b/python/src/mrpt/obs/CSensoryFrame.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/mrpt/opengl/COctoMapVoxels.cpp b/python/src/mrpt/opengl/COctoMapVoxels.cpp index 722a508176..aab8ee5493 100644 --- a/python/src/mrpt/opengl/COctoMapVoxels.cpp +++ b/python/src/mrpt/opengl/COctoMapVoxels.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -60,7 +61,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::opengl::COctoMapVoxels file:mrpt/opengl/COctoMapVoxels.h line:63 +// mrpt::opengl::COctoMapVoxels file:mrpt/opengl/COctoMapVoxels.h line:64 struct PyCallBack_mrpt_opengl_COctoMapVoxels : public mrpt::opengl::COctoMapVoxels { using mrpt::opengl::COctoMapVoxels::COctoMapVoxels; @@ -315,7 +316,7 @@ struct PyCallBack_mrpt_opengl_COctoMapVoxels : public mrpt::opengl::COctoMapVoxe void bind_mrpt_opengl_COctoMapVoxels(std::function< pybind11::module &(std::string const &namespace_) > &M) { - // mrpt::opengl::predefined_voxel_sets_t file:mrpt/opengl/COctoMapVoxels.h line:18 + // mrpt::opengl::predefined_voxel_sets_t file:mrpt/opengl/COctoMapVoxels.h line:19 pybind11::enum_(M("mrpt::opengl"), "predefined_voxel_sets_t", pybind11::arithmetic(), "") .value("VOXEL_SET_OCCUPIED", mrpt::opengl::VOXEL_SET_OCCUPIED) .value("VOXEL_SET_FREESPACE", mrpt::opengl::VOXEL_SET_FREESPACE) @@ -323,7 +324,7 @@ void bind_mrpt_opengl_COctoMapVoxels(std::function< pybind11::module &(std::stri ; - { // mrpt::opengl::COctoMapVoxels file:mrpt/opengl/COctoMapVoxels.h line:63 + { // mrpt::opengl::COctoMapVoxels file:mrpt/opengl/COctoMapVoxels.h line:64 pybind11::class_, PyCallBack_mrpt_opengl_COctoMapVoxels, mrpt::opengl::CRenderizableShaderTriangles, mrpt::opengl::CRenderizableShaderWireFrame, mrpt::opengl::CRenderizableShaderPoints> cl(M("mrpt::opengl"), "COctoMapVoxels", "A flexible renderer of voxels, typically from a 3D octo map (see\nmrpt::maps::COctoMap).\n This class is sort of equivalent to octovis::OcTreeDrawer from the octomap\npackage, but\n relying on MRPT's CRenderizable so there's no need to manually\ncache the rendering of OpenGL primitives.\n\n Normally users call mrpt::maps::COctoMap::getAs3DObject() to obtain a\ngeneric mrpt::opengl::CSetOfObjects which insides holds an instance of\nCOctoMapVoxels.\n You can also alternativelly call COctoMapVoxels::setFromOctoMap(), so you\ncan tune the display parameters, colors, etc.\n As with any other mrpt::opengl class, all object coordinates refer to some\nframe of reference which is relative to the object parent and can be changed\nwith mrpt::opengl::CRenderizable::setPose()\n\n This class draws these separate elements to represent an OctoMap:\n - A grid representation of all cubes, as simple lines (occupied/free,\nleafs/nodes,... whatever). See:\n - showGridLines()\n - setGridLinesColor()\n - setGridLinesWidth()\n - push_back_GridCube()\n - A number of voxel collections, drawn as cubes each having a\ndifferent color (e.g. depending on the color scheme in the original\nmrpt::maps::COctoMap object).\n The meanning of each collection is user-defined, but you can use the\nconstants VOXEL_SET_OCCUPIED, VOXEL_SET_FREESPACE for predefined meanings.\n - showVoxels()\n - push_back_Voxel()\n\n Several coloring schemes can be selected with setVisualizationMode(). See\nCOctoMapVoxels::visualization_mode_t\n\n ![mrpt::opengl::COctoMapVoxels](preview_COctoMapVoxels.png)\n\n \n opengl::Scene\n \n\n\n "); cl.def( pybind11::init( [](){ return new mrpt::opengl::COctoMapVoxels(); }, [](){ return new PyCallBack_mrpt_opengl_COctoMapVoxels(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_opengl_COctoMapVoxels const &o){ return new PyCallBack_mrpt_opengl_COctoMapVoxels(o); } ) ); @@ -344,6 +345,8 @@ void bind_mrpt_opengl_COctoMapVoxels(std::function< pybind11::module &(std::stri cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::opengl::COctoMapVoxels::*)() const) &mrpt::opengl::COctoMapVoxels::GetRuntimeClass, "C++: mrpt::opengl::COctoMapVoxels::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); cl.def("clone", (class mrpt::rtti::CObject * (mrpt::opengl::COctoMapVoxels::*)() const) &mrpt::opengl::COctoMapVoxels::clone, "C++: mrpt::opengl::COctoMapVoxels::clone() const --> class mrpt::rtti::CObject *", pybind11::return_value_policy::automatic); cl.def_static("CreateObject", (class std::shared_ptr (*)()) &mrpt::opengl::COctoMapVoxels::CreateObject, "C++: mrpt::opengl::COctoMapVoxels::CreateObject() --> class std::shared_ptr"); + cl.def("colorMap", (enum mrpt::img::TColormap (mrpt::opengl::COctoMapVoxels::*)() const) &mrpt::opengl::COctoMapVoxels::colorMap, "C++: mrpt::opengl::COctoMapVoxels::colorMap() const --> enum mrpt::img::TColormap"); + cl.def("colorMap", (void (mrpt::opengl::COctoMapVoxels::*)(const enum mrpt::img::TColormap &)) &mrpt::opengl::COctoMapVoxels::colorMap, "Changing the colormap has no effect until a source object (e.g.\n mrpt::maps::CVoxelMap) reads this property while generating the voxels\n visualization.\n\nC++: mrpt::opengl::COctoMapVoxels::colorMap(const enum mrpt::img::TColormap &) --> void", pybind11::arg("cm")); cl.def("renderUpdateBuffers", (void (mrpt::opengl::COctoMapVoxels::*)() const) &mrpt::opengl::COctoMapVoxels::renderUpdateBuffers, "C++: mrpt::opengl::COctoMapVoxels::renderUpdateBuffers() const --> void"); cl.def("onUpdateBuffers_Points", (void (mrpt::opengl::COctoMapVoxels::*)()) &mrpt::opengl::COctoMapVoxels::onUpdateBuffers_Points, "C++: mrpt::opengl::COctoMapVoxels::onUpdateBuffers_Points() --> void"); cl.def("onUpdateBuffers_Wireframe", (void (mrpt::opengl::COctoMapVoxels::*)()) &mrpt::opengl::COctoMapVoxels::onUpdateBuffers_Wireframe, "C++: mrpt::opengl::COctoMapVoxels::onUpdateBuffers_Wireframe() --> void"); @@ -387,7 +390,7 @@ void bind_mrpt_opengl_COctoMapVoxels(std::function< pybind11::module &(std::stri cl.def("internalBoundingBoxLocal", (struct mrpt::math::TBoundingBox_ (mrpt::opengl::COctoMapVoxels::*)() const) &mrpt::opengl::COctoMapVoxels::internalBoundingBoxLocal, "C++: mrpt::opengl::COctoMapVoxels::internalBoundingBoxLocal() const --> struct mrpt::math::TBoundingBox_"); cl.def("assign", (class mrpt::opengl::COctoMapVoxels & (mrpt::opengl::COctoMapVoxels::*)(const class mrpt::opengl::COctoMapVoxels &)) &mrpt::opengl::COctoMapVoxels::operator=, "C++: mrpt::opengl::COctoMapVoxels::operator=(const class mrpt::opengl::COctoMapVoxels &) --> class mrpt::opengl::COctoMapVoxels &", pybind11::return_value_policy::automatic, pybind11::arg("")); - { // mrpt::opengl::COctoMapVoxels::TVoxel file:mrpt/opengl/COctoMapVoxels.h line:95 + { // mrpt::opengl::COctoMapVoxels::TVoxel file:mrpt/opengl/COctoMapVoxels.h line:96 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TVoxel", "The info of each of the voxels "); cl.def( pybind11::init( [](){ return new mrpt::opengl::COctoMapVoxels::TVoxel(); } ) ); @@ -400,7 +403,7 @@ void bind_mrpt_opengl_COctoMapVoxels(std::function< pybind11::module &(std::stri cl.def("assign", (struct mrpt::opengl::COctoMapVoxels::TVoxel & (mrpt::opengl::COctoMapVoxels::TVoxel::*)(const struct mrpt::opengl::COctoMapVoxels::TVoxel &)) &mrpt::opengl::COctoMapVoxels::TVoxel::operator=, "C++: mrpt::opengl::COctoMapVoxels::TVoxel::operator=(const struct mrpt::opengl::COctoMapVoxels::TVoxel &) --> struct mrpt::opengl::COctoMapVoxels::TVoxel &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::opengl::COctoMapVoxels::TGridCube file:mrpt/opengl/COctoMapVoxels.h line:111 + { // mrpt::opengl::COctoMapVoxels::TGridCube file:mrpt/opengl/COctoMapVoxels.h line:112 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TGridCube", "The info of each grid block "); cl.def( pybind11::init( [](){ return new mrpt::opengl::COctoMapVoxels::TGridCube(); } ) ); @@ -412,7 +415,7 @@ void bind_mrpt_opengl_COctoMapVoxels(std::function< pybind11::module &(std::stri cl.def("assign", (struct mrpt::opengl::COctoMapVoxels::TGridCube & (mrpt::opengl::COctoMapVoxels::TGridCube::*)(const struct mrpt::opengl::COctoMapVoxels::TGridCube &)) &mrpt::opengl::COctoMapVoxels::TGridCube::operator=, "C++: mrpt::opengl::COctoMapVoxels::TGridCube::operator=(const struct mrpt::opengl::COctoMapVoxels::TGridCube &) --> struct mrpt::opengl::COctoMapVoxels::TGridCube &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::opengl::COctoMapVoxels::TInfoPerVoxelSet file:mrpt/opengl/COctoMapVoxels.h line:125 + { // mrpt::opengl::COctoMapVoxels::TInfoPerVoxelSet file:mrpt/opengl/COctoMapVoxels.h line:126 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TInfoPerVoxelSet", ""); cl.def( pybind11::init( [](){ return new mrpt::opengl::COctoMapVoxels::TInfoPerVoxelSet(); } ) ); diff --git a/python/src/mrpt/poses/CPointPDF.cpp b/python/src/mrpt/poses/CPointPDF.cpp index cf84e9ea6e..7d4d5fb393 100644 --- a/python/src/mrpt/poses/CPointPDF.cpp +++ b/python/src/mrpt/poses/CPointPDF.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/mrpt/poses/CPose3DPDFParticles.cpp b/python/src/mrpt/poses/CPose3DPDFParticles.cpp index 67c696a5b8..be9d63928f 100644 --- a/python/src/mrpt/poses/CPose3DPDFParticles.cpp +++ b/python/src/mrpt/poses/CPose3DPDFParticles.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/mrpt/rtti/CObject.cpp b/python/src/mrpt/rtti/CObject.cpp index 0918d66668..3a3e3c4d81 100644 --- a/python/src/mrpt/rtti/CObject.cpp +++ b/python/src/mrpt/rtti/CObject.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/mrpt/slam/CMetricMapsAlignmentAlgorithm.cpp b/python/src/mrpt/slam/CMetricMapsAlignmentAlgorithm.cpp index 093174d501..277fd3d6c8 100644 --- a/python/src/mrpt/slam/CMetricMapsAlignmentAlgorithm.cpp +++ b/python/src/mrpt/slam/CMetricMapsAlignmentAlgorithm.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/mrpt/slam/CMonteCarloLocalization3D.cpp b/python/src/mrpt/slam/CMonteCarloLocalization3D.cpp index 9882a891e5..12246f3a32 100644 --- a/python/src/mrpt/slam/CMonteCarloLocalization3D.cpp +++ b/python/src/mrpt/slam/CMonteCarloLocalization3D.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/mrpt/slam/COccupancyGridMapFeatureExtractor.cpp b/python/src/mrpt/slam/COccupancyGridMapFeatureExtractor.cpp index f3e5880e92..bb9779cb57 100644 --- a/python/src/mrpt/slam/COccupancyGridMapFeatureExtractor.cpp +++ b/python/src/mrpt/slam/COccupancyGridMapFeatureExtractor.cpp @@ -16,7 +16,9 @@ #include #include #include +#include #include +#include #include #include #include diff --git a/python/src/mrpt/slam/CRejectionSamplingRangeOnlyLocalization.cpp b/python/src/mrpt/slam/CRejectionSamplingRangeOnlyLocalization.cpp index 39cf9486e7..857b10c79a 100644 --- a/python/src/mrpt/slam/CRejectionSamplingRangeOnlyLocalization.cpp +++ b/python/src/mrpt/slam/CRejectionSamplingRangeOnlyLocalization.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/mrpt/slam/TKLDParams.cpp b/python/src/mrpt/slam/TKLDParams.cpp index 944fc93f99..505fcd4481 100644 --- a/python/src/mrpt/slam/TKLDParams.cpp +++ b/python/src/mrpt/slam/TKLDParams.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/pymrpt.cpp b/python/src/pymrpt.cpp index e4a28636a0..b7e61382cf 100644 --- a/python/src/pymrpt.cpp +++ b/python/src/pymrpt.cpp @@ -86,6 +86,7 @@ void bind_mrpt_obs_CObservation(std::function< pybind11::module &(std::string co void bind_mrpt_system_mrptEvent(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_maps_CMetricMapEvents(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_maps_metric_map_types(std::function< pybind11::module &(std::string const &namespace_) > &M); +void bind_mrpt_math_TBoundingBox(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_opengl_Visualizable(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_system_CObservable(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_tfest_TMatchingPair(std::function< pybind11::module &(std::string const &namespace_) > &M); @@ -110,7 +111,7 @@ void bind_mrpt_bayes_CParticleFilterData(std::function< pybind11::module &(std:: void bind_mrpt_bayes_CParticleFilterData_1(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_poses_CPose3DPDFParticles(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_img_color_maps(std::function< pybind11::module &(std::string const &namespace_) > &M); -void bind_mrpt_math_TBoundingBox(std::function< pybind11::module &(std::string const &namespace_) > &M); +void bind_mrpt_maps_NearestNeighborsCapable(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_obs_T2DScanProperties(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_opengl_PLY_import_export(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_maps_CPointsMap(std::function< pybind11::module &(std::string const &namespace_) > &M); @@ -514,6 +515,7 @@ PYBIND11_MODULE(pymrpt, root_module) { bind_mrpt_system_mrptEvent(M); bind_mrpt_maps_CMetricMapEvents(M); bind_mrpt_maps_metric_map_types(M); + bind_mrpt_math_TBoundingBox(M); bind_mrpt_opengl_Visualizable(M); bind_mrpt_system_CObservable(M); bind_mrpt_tfest_TMatchingPair(M); @@ -538,7 +540,7 @@ PYBIND11_MODULE(pymrpt, root_module) { bind_mrpt_bayes_CParticleFilterData_1(M); bind_mrpt_poses_CPose3DPDFParticles(M); bind_mrpt_img_color_maps(M); - bind_mrpt_math_TBoundingBox(M); + bind_mrpt_maps_NearestNeighborsCapable(M); bind_mrpt_obs_T2DScanProperties(M); bind_mrpt_opengl_PLY_import_export(M); bind_mrpt_maps_CPointsMap(M); diff --git a/python/src/pymrpt.sources b/python/src/pymrpt.sources index ae6dd7aa12..293c3ffab3 100644 --- a/python/src/pymrpt.sources +++ b/python/src/pymrpt.sources @@ -76,6 +76,7 @@ mrpt/obs/CObservation.cpp mrpt/system/mrptEvent.cpp mrpt/maps/CMetricMapEvents.cpp mrpt/maps/metric_map_types.cpp +mrpt/math/TBoundingBox.cpp mrpt/opengl/Visualizable.cpp mrpt/system/CObservable.cpp mrpt/tfest/TMatchingPair.cpp @@ -100,7 +101,7 @@ mrpt/bayes/CParticleFilterData.cpp mrpt/bayes/CParticleFilterData_1.cpp mrpt/poses/CPose3DPDFParticles.cpp mrpt/img/color_maps.cpp -mrpt/math/TBoundingBox.cpp +mrpt/maps/NearestNeighborsCapable.cpp mrpt/obs/T2DScanProperties.cpp mrpt/opengl/PLY_import_export.cpp mrpt/maps/CPointsMap.cpp diff --git a/python/src/unknown/unknown_6.cpp b/python/src/unknown/unknown_6.cpp index 05fb332486..4a99579891 100644 --- a/python/src/unknown/unknown_6.cpp +++ b/python/src/unknown/unknown_6.cpp @@ -115,7 +115,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_GENERIC_SHORT_FRAME : public mrp } }; -// mrpt::obs::gnss::Message_NV_OEM6_BESTPOS file: line:86 +// mrpt::obs::gnss::Message_NV_OEM6_BESTPOS file: line:82 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_BESTPOS : public mrpt::obs::gnss::Message_NV_OEM6_BESTPOS { using mrpt::obs::gnss::Message_NV_OEM6_BESTPOS::Message_NV_OEM6_BESTPOS; @@ -160,7 +160,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_BESTPOS : public mrpt::obs::gnss } }; -// mrpt::obs::gnss::Message_NV_OEM6_INSPVAS file: line:92 +// mrpt::obs::gnss::Message_NV_OEM6_INSPVAS file: line:88 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSPVAS : public mrpt::obs::gnss::Message_NV_OEM6_INSPVAS { using mrpt::obs::gnss::Message_NV_OEM6_INSPVAS::Message_NV_OEM6_INSPVAS; @@ -205,7 +205,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSPVAS : public mrpt::obs::gnss } }; -// mrpt::obs::gnss::Message_NV_OEM6_INSCOVS file: line:98 +// mrpt::obs::gnss::Message_NV_OEM6_INSCOVS file: line:94 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSCOVS : public mrpt::obs::gnss::Message_NV_OEM6_INSCOVS { using mrpt::obs::gnss::Message_NV_OEM6_INSCOVS::Message_NV_OEM6_INSCOVS; @@ -317,7 +317,7 @@ void bind_unknown_unknown_6(std::function< pybind11::module &(std::string const cl.def("fixEndianness", (void (mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME::*)()) &mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME::fixEndianness, "C++: mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME::fixEndianness() --> void"); cl.def("assign", (struct mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME & (mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME::*)(const struct mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME &)) &mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME::operator=, "C++: mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME::operator=(const struct mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME &) --> struct mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::obs::gnss::Message_NV_OEM6_BESTPOS file: line:86 + { // mrpt::obs::gnss::Message_NV_OEM6_BESTPOS file: line:82 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_BESTPOS, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_BESTPOS", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_BESTPOS(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_BESTPOS(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_BESTPOS const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_BESTPOS(o); } ) ); @@ -357,7 +357,7 @@ void bind_unknown_unknown_6(std::function< pybind11::module &(std::string const } } - { // mrpt::obs::gnss::Message_NV_OEM6_INSPVAS file: line:92 + { // mrpt::obs::gnss::Message_NV_OEM6_INSPVAS file: line:88 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSPVAS, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_INSPVAS", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_INSPVAS(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSPVAS(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSPVAS const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSPVAS(o); } ) ); @@ -389,7 +389,7 @@ void bind_unknown_unknown_6(std::function< pybind11::module &(std::string const } } - { // mrpt::obs::gnss::Message_NV_OEM6_INSCOVS file: line:98 + { // mrpt::obs::gnss::Message_NV_OEM6_INSCOVS file: line:94 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSCOVS, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_INSCOVS", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_INSCOVS(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSCOVS(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSCOVS const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSCOVS(o); } ) ); diff --git a/python/src/unknown/unknown_7.cpp b/python/src/unknown/unknown_7.cpp index 8cbcc882ef..6955b09b24 100644 --- a/python/src/unknown/unknown_7.cpp +++ b/python/src/unknown/unknown_7.cpp @@ -25,7 +25,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::obs::gnss::Message_NV_OEM6_RXSTATUS file: line:104 +// mrpt::obs::gnss::Message_NV_OEM6_RXSTATUS file: line:100 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RXSTATUS : public mrpt::obs::gnss::Message_NV_OEM6_RXSTATUS { using mrpt::obs::gnss::Message_NV_OEM6_RXSTATUS::Message_NV_OEM6_RXSTATUS; @@ -70,7 +70,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RXSTATUS : public mrpt::obs::gns } }; -// mrpt::obs::gnss::Message_NV_OEM6_RAWEPHEM file: line:110 +// mrpt::obs::gnss::Message_NV_OEM6_RAWEPHEM file: line:106 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWEPHEM : public mrpt::obs::gnss::Message_NV_OEM6_RAWEPHEM { using mrpt::obs::gnss::Message_NV_OEM6_RAWEPHEM::Message_NV_OEM6_RAWEPHEM; @@ -160,7 +160,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_VERSION : public mrpt::obs::gnss } }; -// mrpt::obs::gnss::Message_NV_OEM6_RAWIMUS file: line:116 +// mrpt::obs::gnss::Message_NV_OEM6_RAWIMUS file: line:112 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWIMUS : public mrpt::obs::gnss::Message_NV_OEM6_RAWIMUS { using mrpt::obs::gnss::Message_NV_OEM6_RAWIMUS::Message_NV_OEM6_RAWIMUS; @@ -205,7 +205,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWIMUS : public mrpt::obs::gnss } }; -// mrpt::obs::gnss::Message_NV_OEM6_MARKPOS file: line:122 +// mrpt::obs::gnss::Message_NV_OEM6_MARKPOS file: line:118 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKPOS : public mrpt::obs::gnss::Message_NV_OEM6_MARKPOS { using mrpt::obs::gnss::Message_NV_OEM6_MARKPOS::Message_NV_OEM6_MARKPOS; @@ -252,7 +252,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKPOS : public mrpt::obs::gnss void bind_unknown_unknown_7(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::obs::gnss::Message_NV_OEM6_RXSTATUS file: line:104 + { // mrpt::obs::gnss::Message_NV_OEM6_RXSTATUS file: line:100 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RXSTATUS, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_RXSTATUS", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_RXSTATUS(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RXSTATUS(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RXSTATUS const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RXSTATUS(o); } ) ); @@ -290,7 +290,7 @@ void bind_unknown_unknown_7(std::function< pybind11::module &(std::string const } } - { // mrpt::obs::gnss::Message_NV_OEM6_RAWEPHEM file: line:110 + { // mrpt::obs::gnss::Message_NV_OEM6_RAWEPHEM file: line:106 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWEPHEM, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_RAWEPHEM", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_RAWEPHEM(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWEPHEM(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWEPHEM const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWEPHEM(o); } ) ); @@ -333,7 +333,7 @@ void bind_unknown_unknown_7(std::function< pybind11::module &(std::string const } } - { // mrpt::obs::gnss::Message_NV_OEM6_RAWIMUS file: line:116 + { // mrpt::obs::gnss::Message_NV_OEM6_RAWIMUS file: line:112 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWIMUS, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_RAWIMUS", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_RAWIMUS(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWIMUS(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWIMUS const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWIMUS(o); } ) ); @@ -362,7 +362,7 @@ void bind_unknown_unknown_7(std::function< pybind11::module &(std::string const } } - { // mrpt::obs::gnss::Message_NV_OEM6_MARKPOS file: line:122 + { // mrpt::obs::gnss::Message_NV_OEM6_MARKPOS file: line:118 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKPOS, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_MARKPOS", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_MARKPOS(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKPOS(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKPOS const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKPOS(o); } ) ); diff --git a/python/src/unknown/unknown_8.cpp b/python/src/unknown/unknown_8.cpp index a475ccb6c0..ac4aaccbb1 100644 --- a/python/src/unknown/unknown_8.cpp +++ b/python/src/unknown/unknown_8.cpp @@ -25,7 +25,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::obs::gnss::Message_NV_OEM6_MARKTIME file: line:128 +// mrpt::obs::gnss::Message_NV_OEM6_MARKTIME file: line:124 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKTIME : public mrpt::obs::gnss::Message_NV_OEM6_MARKTIME { using mrpt::obs::gnss::Message_NV_OEM6_MARKTIME::Message_NV_OEM6_MARKTIME; @@ -70,7 +70,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKTIME : public mrpt::obs::gns } }; -// mrpt::obs::gnss::Message_NV_OEM6_MARK2TIME file: line:5 +// mrpt::obs::gnss::Message_NV_OEM6_MARK2TIME file: line:130 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARK2TIME : public mrpt::obs::gnss::Message_NV_OEM6_MARK2TIME { using mrpt::obs::gnss::Message_NV_OEM6_MARK2TIME::Message_NV_OEM6_MARK2TIME; @@ -115,7 +115,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARK2TIME : public mrpt::obs::gn } }; -// mrpt::obs::gnss::Message_NV_OEM6_IONUTC file: line:11 +// mrpt::obs::gnss::Message_NV_OEM6_IONUTC file: line:6 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_IONUTC : public mrpt::obs::gnss::Message_NV_OEM6_IONUTC { using mrpt::obs::gnss::Message_NV_OEM6_IONUTC::Message_NV_OEM6_IONUTC; @@ -252,7 +252,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_TOPCON_SATS : public mrpt::obs::gnss::Me void bind_unknown_unknown_8(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::obs::gnss::Message_NV_OEM6_MARKTIME file: line:128 + { // mrpt::obs::gnss::Message_NV_OEM6_MARKTIME file: line:124 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKTIME, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_MARKTIME", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_MARKTIME(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKTIME(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKTIME const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKTIME(o); } ) ); @@ -278,7 +278,7 @@ void bind_unknown_unknown_8(std::function< pybind11::module &(std::string const } } - { // mrpt::obs::gnss::Message_NV_OEM6_MARK2TIME file: line:5 + { // mrpt::obs::gnss::Message_NV_OEM6_MARK2TIME file: line:130 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARK2TIME, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_MARK2TIME", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_MARK2TIME(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARK2TIME(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARK2TIME const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARK2TIME(o); } ) ); @@ -304,7 +304,7 @@ void bind_unknown_unknown_8(std::function< pybind11::module &(std::string const } } - { // mrpt::obs::gnss::Message_NV_OEM6_IONUTC file: line:11 + { // mrpt::obs::gnss::Message_NV_OEM6_IONUTC file: line:6 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_IONUTC, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_IONUTC", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_IONUTC(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_IONUTC(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_IONUTC const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_IONUTC(o); } ) ); diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi index 6145d80537..c1ffc6a3d2 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi @@ -1145,7 +1145,10 @@ class CLogOddsGridMap2D_signed_char_t: class CLogOddsGridMap3D_signed_char_t: m_grid: mrpt.pymrpt.mrpt.containers.CDynamicGrid3D_signed_char_double_t + @overload def __init__(self) -> None: ... + @overload + def __init__(self, arg0: CLogOddsGridMap3D_signed_char_t) -> None: ... def assign(self) -> CLogOddsGridMap3D_signed_char_t: ... @overload def updateCell_fast_free(self, theCell: int, logodd_obs: int, thres: int) -> None: ... @@ -1188,6 +1191,7 @@ class CMetricMap(mrpt.pymrpt.mrpt.serialization.CSerializable, mrpt.pymrpt.mrpt. def auxParticleFilterCleanUp(self) -> None: ... @overload def auxParticleFilterCleanUp() -> void: ... + def boundingBox(self) -> mrpt.pymrpt.mrpt.math.TBoundingBox_float_t: ... @overload def canComputeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation) -> bool: ... @overload @@ -1369,7 +1373,7 @@ class CMultiMetricMapPDF(mrpt.pymrpt.mrpt.serialization.CSerializable, mrpt.pymr @overload def updateSensoryFrameSequence() -> void: ... -class COccupancyGridMap2D(CMetricMap, CLogOddsGridMap2D_signed_char_t): +class COccupancyGridMap2D(CMetricMap, CLogOddsGridMap2D_signed_char_t, NearestNeighborsCapable): class TCriticalPointsList: clearance: List[int] x: List[int] @@ -1605,6 +1609,7 @@ class COccupancyGridMap2D(CMetricMap, CLogOddsGridMap2D_signed_char_t): def GetRuntimeClassIdStatic(self, *args, **kwargs) -> Any: ... def asString(self) -> str: ... def assign(self) -> COccupancyGridMap2D: ... + def boundingBox(self) -> mrpt.pymrpt.mrpt.math.TBoundingBox_float_t: ... @overload def buildVoronoiDiagram(self, threshold: float, robot_size: float) -> None: ... @overload @@ -1748,6 +1753,7 @@ class COccupancyGridMap2D(CMetricMap, CLogOddsGridMap2D_signed_char_t): @overload def loadFromBitmapFile(conststd, float, conststructmrpt) -> bool: ... def loadFromROSMapServerYAML(self, yamlFilePath: str) -> bool: ... + def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... def p2l(self, *args, **kwargs) -> Any: ... @overload def resizeGrid(self, new_x_min: float, new_x_max: float, new_y_min: float, new_y_max: float) -> None: ... @@ -1807,13 +1813,14 @@ class COccupancyGridMap2D(CMetricMap, CLogOddsGridMap2D_signed_char_t): @overload def y2idx(self, y: float, ymin: float) -> int: ... -class COccupancyGridMap3D(CMetricMap, CLogOddsGridMap3D_signed_char_t): +class COccupancyGridMap3D(CMetricMap, CLogOddsGridMap3D_signed_char_t, NearestNeighborsCapable): class TInsertionOptions(mrpt.pymrpt.mrpt.config.CLoadableOptions): decimation: int decimation_3d_range: int maxDistanceInsertion: float maxFreenessUpdateCertainty: float maxOccupancyUpdateCertainty: float + raytraceEmptyCells: bool @overload def __init__(self) -> None: ... @overload @@ -1908,7 +1915,10 @@ class COccupancyGridMap3D(CMetricMap, CLogOddsGridMap3D_signed_char_t): generateOccupiedVoxels: bool visibleFreeVoxels: bool visibleOccupiedVoxels: bool + @overload def __init__(self) -> None: ... + @overload + def __init__(self, arg0: COccupancyGridMap3D.TRenderingOptions) -> None: ... def assign(self) -> COccupancyGridMap3D.TRenderingOptions: ... @overload def readFromStream(self, in: mrpt.pymrpt.mrpt.serialization.CArchive) -> None: ... @@ -1931,11 +1941,16 @@ class COccupancyGridMap3D(CMetricMap, CLogOddsGridMap3D_signed_char_t): def __init__(self, arg0, arg1) -> None: ... @overload def __init__(self, corner_min, corner_max, resolution: float) -> None: ... + @overload + def __init__(self, arg0: COccupancyGridMap3D) -> None: ... + @overload + def __init__(self, arg0: COccupancyGridMap3D) -> None: ... def CreateObject(self, *args, **kwargs) -> Any: ... def GetRuntimeClass(self) -> mrpt.pymrpt.mrpt.rtti.TRuntimeClassId: ... def GetRuntimeClassIdStatic(self, *args, **kwargs) -> Any: ... def asString(self) -> str: ... def assign(self) -> COccupancyGridMap3D: ... + def boundingBox(self) -> mrpt.pymrpt.mrpt.math.TBoundingBox_float_t: ... def clone(self) -> mrpt.pymrpt.mrpt.rtti.CObject: ... def compute3DMatchingRatio(self, otherMap: CMetricMap, otherMapPose: mrpt.pymrpt.mrpt.poses.CPose3D, params: TMatchingRatioParams) -> float: ... def determineMatching2D(self, otherMap: CMetricMap, otherMapPose: mrpt.pymrpt.mrpt.poses.CPose2D, correspondences: mrpt.pymrpt.mrpt.tfest.TMatchingPairListTempl_float_t, params: TMatchingParams, extraResults: TMatchingExtraResults) -> None: ... @@ -1956,18 +1971,13 @@ class COccupancyGridMap3D(CMetricMap, CLogOddsGridMap3D_signed_char_t): @overload def getVisualizationInto(classmrpt) -> void: ... @overload - def insertPointCloud(self, sensorCenter, pts: CPointsMap) -> None: ... - @overload - def insertPointCloud(self, sensorCenter, pts: CPointsMap, maxValidRange: float) -> None: ... - @overload - def insertPointCloud(conststructmrpt, constclassmrpt, constfloat) -> void: ... - @overload def insertRay(self, sensor, end) -> None: ... @overload def insertRay(self, sensor, end, endIsOccupied: bool) -> None: ... def isEmpty(self, *args, **kwargs) -> Any: ... def l2p(self, *args, **kwargs) -> Any: ... def l2p_255(self, *args, **kwargs) -> Any: ... + def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... def p2l(self, *args, **kwargs) -> Any: ... @overload def resizeGrid(self, corner_min, corner_max) -> None: ... @@ -2258,6 +2268,7 @@ class COctoMapBase_octomap_ColorOcTree_octomap_ColorOcTreeNode_t(CMetricMap): def auxParticleFilterCleanUp(self) -> None: ... @overload def auxParticleFilterCleanUp() -> void: ... + def boundingBox(self) -> mrpt.pymrpt.mrpt.math.TBoundingBox_float_t: ... @overload def canComputeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation) -> bool: ... @overload @@ -2517,6 +2528,7 @@ class COctoMapBase_octomap_OcTree_octomap_OcTreeNode_t(CMetricMap): def auxParticleFilterCleanUp(self) -> None: ... @overload def auxParticleFilterCleanUp() -> void: ... + def boundingBox(self) -> mrpt.pymrpt.mrpt.math.TBoundingBox_float_t: ... @overload def canComputeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation) -> bool: ... @overload @@ -2694,7 +2706,7 @@ class CPointCloudFilterByDistance(CPointCloudFilterBase): @overload def filter(classmrpt, constmrpt, constclassmrpt, structmrpt) -> void: ... -class CPointsMap(CMetricMap, mrpt.pymrpt.mrpt.opengl.PLY_Importer, mrpt.pymrpt.mrpt.opengl.PLY_Exporter): +class CPointsMap(CMetricMap, mrpt.pymrpt.mrpt.opengl.PLY_Importer, mrpt.pymrpt.mrpt.opengl.PLY_Exporter, NearestNeighborsCapable): class TInsertionOptions(mrpt.pymrpt.mrpt.config.CLoadableOptions): addToExistingPointsMap: bool also_interpolate: bool @@ -2907,6 +2919,7 @@ class CPointsMap(CMetricMap, mrpt.pymrpt.mrpt.opengl.PLY_Importer, mrpt.pymrpt.m def mark_as_modified(self) -> None: ... @overload def mark_as_modified() -> void: ... + def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... @overload def reserve(self, newLength: int) -> None: ... @overload @@ -3625,6 +3638,7 @@ class CVoxelMapBase_mrpt_maps_VoxelNodeOccRGB_t(CMetricMap): def auxParticleFilterCleanUp(self) -> None: ... @overload def auxParticleFilterCleanUp() -> void: ... + def boundingBox(self) -> mrpt.pymrpt.mrpt.math.TBoundingBox_float_t: ... @overload def canComputeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation) -> bool: ... @overload @@ -3695,6 +3709,7 @@ class CVoxelMapBase_mrpt_maps_VoxelNodeOccupancy_t(CMetricMap): def auxParticleFilterCleanUp(self) -> None: ... @overload def auxParticleFilterCleanUp() -> void: ... + def boundingBox(self) -> mrpt.pymrpt.mrpt.math.TBoundingBox_float_t: ... @overload def canComputeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation) -> bool: ... @overload @@ -3751,7 +3766,7 @@ class CVoxelMapBase_mrpt_maps_VoxelNodeOccupancy_t(CMetricMap): def saveMetricMapRepresentationToFile(conststd) -> void: ... def squareDistanceToClosestCorrespondence(self, x0: float, y0: float) -> float: ... -class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t(CVoxelMapBase_mrpt_maps_VoxelNodeOccRGB_t): +class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t(CVoxelMapBase_mrpt_maps_VoxelNodeOccRGB_t, NearestNeighborsCapable): genericMapParams: TMapGenericParams insertionOptions: TVoxelMap_InsertionOptions likelihoodOptions: TVoxelMap_LikelihoodOptions @@ -3767,10 +3782,16 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t(CVoxelMapBa @overload def assign(self) -> CMetricMap: ... @overload + def assign(self) -> NearestNeighborsCapable: ... + @overload def auxParticleFilterCleanUp(self) -> None: ... @overload def auxParticleFilterCleanUp() -> void: ... @overload + def boundingBox(self) -> mrpt.pymrpt.mrpt.math.TBoundingBox_float_t: ... + @overload + def boundingBox(self) -> mrpt.pymrpt.mrpt.math.TBoundingBox_float_t: ... + @overload def canComputeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation) -> bool: ... @overload def canComputeObservationLikelihood(constclassmrpt) -> bool: ... @@ -3812,14 +3833,6 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t(CVoxelMapBa @overload def insertObs(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame, robotPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> bool: ... @overload - def insertPointCloudAsEndPoints(self, pts: CPointsMap, sensorPt) -> None: ... - @overload - def insertPointCloudAsEndPoints(constclassmrpt, conststructmrpt) -> void: ... - @overload - def insertPointCloudAsRays(self, pts: CPointsMap, sensorPt) -> None: ... - @overload - def insertPointCloudAsRays(constclassmrpt, conststructmrpt) -> void: ... - @overload def isEmpty(self) -> bool: ... @overload def isEmpty() -> bool: ... @@ -3837,6 +3850,14 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t(CVoxelMapBa def loadFromSimpleMap(self, Map: CSimpleMap) -> None: ... @overload def loadFromSimpleMap(constclassmrpt) -> void: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... def p2l(self, *args, **kwargs) -> Any: ... @overload def saveMetricMapRepresentationToFile(self, filNamePrefix: str) -> None: ... @@ -3851,7 +3872,7 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t(CVoxelMapBa def updateCell_fast_occupied(self, theCell, logodd_obs: int, thres: int) -> None: ... def updateVoxel(self, x: float, y: float, z: float, occupied: bool) -> None: ... -class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t(CVoxelMapBase_mrpt_maps_VoxelNodeOccupancy_t): +class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t(CVoxelMapBase_mrpt_maps_VoxelNodeOccupancy_t, NearestNeighborsCapable): genericMapParams: TMapGenericParams insertionOptions: TVoxelMap_InsertionOptions likelihoodOptions: TVoxelMap_LikelihoodOptions @@ -3867,10 +3888,16 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t(CVoxelMa @overload def assign(self) -> CMetricMap: ... @overload + def assign(self) -> NearestNeighborsCapable: ... + @overload def auxParticleFilterCleanUp(self) -> None: ... @overload def auxParticleFilterCleanUp() -> void: ... @overload + def boundingBox(self) -> mrpt.pymrpt.mrpt.math.TBoundingBox_float_t: ... + @overload + def boundingBox(self) -> mrpt.pymrpt.mrpt.math.TBoundingBox_float_t: ... + @overload def canComputeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation) -> bool: ... @overload def canComputeObservationLikelihood(constclassmrpt) -> bool: ... @@ -3912,14 +3939,6 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t(CVoxelMa @overload def insertObs(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame, robotPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> bool: ... @overload - def insertPointCloudAsEndPoints(self, pts: CPointsMap, sensorPt) -> None: ... - @overload - def insertPointCloudAsEndPoints(constclassmrpt, conststructmrpt) -> void: ... - @overload - def insertPointCloudAsRays(self, pts: CPointsMap, sensorPt) -> None: ... - @overload - def insertPointCloudAsRays(constclassmrpt, conststructmrpt) -> void: ... - @overload def isEmpty(self) -> bool: ... @overload def isEmpty() -> bool: ... @@ -3937,6 +3956,14 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t(CVoxelMa def loadFromSimpleMap(self, Map: CSimpleMap) -> None: ... @overload def loadFromSimpleMap(constclassmrpt) -> void: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... def p2l(self, *args, **kwargs) -> Any: ... @overload def saveMetricMapRepresentationToFile(self, filNamePrefix: str) -> None: ... @@ -4091,6 +4118,14 @@ class CWirelessPowerGridMap2D(CRandomFieldGridMap2D): @overload def getVisualizationInto(classmrpt) -> void: ... +class NearestNeighborsCapable: + def __init__(self, *args, **kwargs) -> None: ... + def assign(self) -> NearestNeighborsCapable: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + class OccGridCellTraits: def __init__(self) -> None: ... diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/math.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/math.pyi index dff77e92e2..84a8af62ce 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/math.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/math.pyi @@ -1669,6 +1669,7 @@ class TBoundingBox_double_t: def name(self) -> str: ... @property def value(self) -> int: ... + __hash__: ClassVar[None] = ... max: Any min: Any @overload @@ -1697,6 +1698,7 @@ class TBoundingBox_double_t: def volume(self) -> float: ... @overload def volume() -> double: ... + def __eq__(self, o: TBoundingBox_double_t) -> bool: ... class TBoundingBox_float_t: class CTOR_FLAGS: @@ -1717,6 +1719,7 @@ class TBoundingBox_float_t: def name(self) -> str: ... @property def value(self) -> int: ... + __hash__: ClassVar[None] = ... max: Any min: Any @overload @@ -1744,6 +1747,7 @@ class TBoundingBox_float_t: def volume(self) -> float: ... @overload def volume() -> float: ... + def __eq__(self, o: TBoundingBox_float_t) -> bool: ... class TConstructorFlags_Matrices: __doc__: ClassVar[str] = ... # read-only @@ -2106,18 +2110,22 @@ class TPoint2D_data_double_t: x: float y: float @overload - def __init__(self, arg0: TPoint2D_data_double_t) -> None: ... - @overload def __init__(self) -> None: ... + @overload + def __init__(self, X: float, Y: float) -> None: ... + @overload + def __init__(self, arg0: TPoint2D_data_double_t) -> None: ... def assign(self) -> TPoint2D_data_double_t: ... class TPoint2D_data_float_t: x: float y: float @overload - def __init__(self, arg0: TPoint2D_data_float_t) -> None: ... - @overload def __init__(self) -> None: ... + @overload + def __init__(self, X: float, Y: float) -> None: ... + @overload + def __init__(self, arg0: TPoint2D_data_float_t) -> None: ... def assign(self) -> TPoint2D_data_float_t: ... class TPoint2D_double_t(TPoseOrPoint, TPoint2D_data_double_t): @@ -2227,9 +2235,11 @@ class TPoint3D_data_double_t: y: float z: float @overload - def __init__(self, arg0: TPoint3D_data_double_t) -> None: ... - @overload def __init__(self) -> None: ... + @overload + def __init__(self, X: float, Y: float, Z: float) -> None: ... + @overload + def __init__(self, arg0: TPoint3D_data_double_t) -> None: ... def assign(self) -> TPoint3D_data_double_t: ... class TPoint3D_data_float_t: @@ -2237,9 +2247,11 @@ class TPoint3D_data_float_t: y: float z: float @overload - def __init__(self, arg0: TPoint3D_data_float_t) -> None: ... - @overload def __init__(self) -> None: ... + @overload + def __init__(self, X: float, Y: float, Z: float) -> None: ... + @overload + def __init__(self, arg0: TPoint3D_data_float_t) -> None: ... def assign(self) -> TPoint3D_data_float_t: ... class TPoint3D_double_t(TPoseOrPoint, TPoint3D_data_double_t): diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi index 06867d0670..b7931b987d 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi @@ -1212,6 +1212,7 @@ class CObservationRange(CObservation): class TMeasurement: sensedDistance: float sensorID: int + sensorNoiseStdDeviation: float sensorPose: mrpt.pymrpt.mrpt.math.TPose3D def __init__(self) -> None: ... maxSensorDistance: float diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi index 17a6c797a4..5bc9a4c2dc 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi @@ -1582,6 +1582,12 @@ class COctoMapVoxels(CRenderizableShaderTriangles, CRenderizableShaderWireFrame, def clear() -> void: ... def clone(self) -> mrpt.pymrpt.mrpt.rtti.CObject: ... @overload + def colorMap(self) -> mrpt.pymrpt.mrpt.img.TColormap: ... + @overload + def colorMap(self, cm: mrpt.pymrpt.mrpt.img.TColormap) -> None: ... + @overload + def colorMap(constenummrpt) -> void: ... + @overload def enableCubeTransparency(self, enable: bool) -> None: ... @overload def enableCubeTransparency(bool) -> void: ... From 591698040fe24385543531946fd80073b88fbbc6 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 5 Nov 2023 10:13:55 +0100 Subject: [PATCH 14/36] NN API: Add support for indices --- .../include/mrpt/maps/COccupancyGridMap2D.h | 20 ++++++++---- .../include/mrpt/maps/COccupancyGridMap3D.h | 21 ++++++++---- libs/maps/include/mrpt/maps/CPointsMap.h | 21 ++++++++---- .../mrpt/maps/CVoxelMapOccupancyBase.h | 32 ++++++++++++------- .../mrpt/maps/NearestNeighborsCapable.h | 32 +++++++++++++++---- .../src/maps/COccupancyGridMap2D_common.cpp | 28 ++++++++++------ .../src/maps/COccupancyGridMap2D_unittest.cpp | 25 +++++++++++---- libs/maps/src/maps/COccupancyGridMap3D.cpp | 20 ++++++++---- .../src/maps/COccupancyGridMap3D_unittest.cpp | 28 +++++++++++----- libs/maps/src/maps/CPointsMap.cpp | 16 ++++++---- 10 files changed, 168 insertions(+), 75 deletions(-) diff --git a/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h b/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h index f91886f849..cb672fd855 100644 --- a/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h +++ b/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h @@ -1173,28 +1173,36 @@ class COccupancyGridMap2D /** @name API of the NearestNeighborsCapable virtual interface @{ */ // See docs in base class + [[nodiscard]] virtual bool nn_supports_indices() const override + { + return false; + } [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr) const override; + float& out_dist_sqr, std::optional& resultIndex) const override; [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr) const override; + float& out_dist_sqr, std::optional& resultIndex) const override; void nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const override; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override; void nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const override; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override; void nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const override; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override; void nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const override; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override; /** @} */ private: diff --git a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h index aadbf50f96..60dba7f215 100644 --- a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h +++ b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h @@ -400,29 +400,36 @@ class COccupancyGridMap3D /** @name API of the NearestNeighborsCapable virtual interface @{ */ - // See docs in base class + [[nodiscard]] virtual bool nn_supports_indices() const override + { + return false; + } [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr) const override; + float& out_dist_sqr, std::optional& resultIndex) const override; [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr) const override; + float& out_dist_sqr, std::optional& resultIndex) const override; void nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const override; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override; void nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const override; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override; void nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const override; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override; void nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const override; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override; /** @} */ private: diff --git a/libs/maps/include/mrpt/maps/CPointsMap.h b/libs/maps/include/mrpt/maps/CPointsMap.h index cdc519584f..4775016c38 100644 --- a/libs/maps/include/mrpt/maps/CPointsMap.h +++ b/libs/maps/include/mrpt/maps/CPointsMap.h @@ -1128,29 +1128,36 @@ class CPointsMap : public CMetricMap, /** @name API of the NearestNeighborsCapable virtual interface @{ */ - // See docs in base class + [[nodiscard]] virtual bool nn_supports_indices() const override + { + return true; + } [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr) const override; + float& out_dist_sqr, std::optional& resultIndex) const override; [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr) const override; + float& out_dist_sqr, std::optional& resultIndex) const override; void nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const override; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override; void nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const override; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override; void nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const override; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override; void nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const override; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override; /** @} */ protected: diff --git a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h index 540dc2b99f..1b4a6927f9 100644 --- a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h +++ b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h @@ -292,51 +292,59 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase, /** @name API of the NearestNeighborsCapable virtual interface @{ */ // See docs in base class + [[nodiscard]] virtual bool nn_supports_indices() const override + { + return getOccupiedVoxels()->nn_supports_indices(); + } [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr) const override + float& out_dist_sqr, std::optional& resultIndex) const override { return getOccupiedVoxels()->nn_single_search( - query, result, out_dist_sqr); + query, result, out_dist_sqr, resultIndex); } [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr) const override + float& out_dist_sqr, std::optional& resultIndex) const override { return getOccupiedVoxels()->nn_single_search( - query, result, out_dist_sqr); + query, result, out_dist_sqr, resultIndex); } void nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const override + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override { getOccupiedVoxels()->nn_multiple_search( - query, N, results, out_dists_sqr); + query, N, results, out_dists_sqr, resultIndices); } void nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const override + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override { getOccupiedVoxels()->nn_multiple_search( - query, N, results, out_dists_sqr); + query, N, results, out_dists_sqr, resultIndices); } void nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const override + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override { getOccupiedVoxels()->nn_radius_search( - query, search_radius_sqr, results, out_dists_sqr); + query, search_radius_sqr, results, out_dists_sqr, resultIndices); } void nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const override + std::vector& out_dists_sqr, + std::optional>& resultIndices) const override { getOccupiedVoxels()->nn_radius_search( - query, search_radius_sqr, results, out_dists_sqr); + query, search_radius_sqr, results, out_dists_sqr, resultIndices); } /** @} */ diff --git a/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h b/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h index 98b4d953e3..d69f04c0bd 100644 --- a/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h +++ b/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h @@ -11,6 +11,8 @@ #include #include +#include + namespace mrpt::maps { /** Virtual interface for maps having the capability of searching the closest @@ -32,23 +34,30 @@ class NearestNeighborsCapable /** @name API of the NearestNeighborsCapable virtual interface @{ */ + /** Returns true if the rest of `nn_*` methods will populate the indices + * std::optional<> return variables, false otherwise. */ + [[nodiscard]] virtual bool nn_supports_indices() const = 0; + /** Search for the closest 3D point to a given one. * * \param[in] query The query input point. * \param[out] result The found closest point. * \param[out] out_dist_sqr The square Euclidean distance between the query * and the returned point. + * \param[out] resultIndex Depending on the implementation, here it will be + * returned the index of the result point in the map, or std::nullopt if + * indices are not available. * * \return True if successful, false if no point was found. */ [[nodiscard]] virtual bool nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr) const = 0; + float& out_dist_sqr, std::optional& resultIndex) const = 0; /// \overload for 2D points [[nodiscard]] virtual bool nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr) const = 0; + float& out_dist_sqr, std::optional& resultIndex) const = 0; /** Search for the `N` closest 3D points to a given one. * @@ -56,17 +65,23 @@ class NearestNeighborsCapable * \param[out] results The found closest points. * \param[out] out_dists_sqr The square Euclidean distances between the * query and the returned point. + * \param[out] resultIndices Depending on the implementation, here it will + * be returned the indices of the result points in the map, or std::nullopt + * if indices are not available. + * */ virtual void nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const = 0; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const = 0; /// \overload for 2D points virtual void nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const = 0; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const = 0; /** Radius search for closest 3D points to a given one. * @@ -75,17 +90,22 @@ class NearestNeighborsCapable * \param[out] results The found closest points. * \param[out] out_dists_sqr The square Euclidean distances between the * query and the returned point. + * \param[out] resultIndices Depending on the implementation, here it will + * be returned the indices of the result points in the map, or std::nullopt + * if indices are not available. */ virtual void nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const = 0; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const = 0; /// \overload for 2D points virtual void nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const = 0; + std::vector& out_dists_sqr, + std::optional>& resultIndices) const = 0; /** @} */ }; diff --git a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp index cf61c9fb75..77bf913f95 100644 --- a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp +++ b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp @@ -770,22 +770,24 @@ float COccupancyGridMap2D::compute3DMatchingRatio( bool COccupancyGridMap2D::nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr) const + float& out_dist_sqr, std::optional& resultIndex) const { // delegate to the 2D version: mrpt::math::TPoint2Df r; - bool res = nn_single_search({query.x, query.y}, r, out_dist_sqr); + bool res = + nn_single_search({query.x, query.y}, r, out_dist_sqr, resultIndex); result = {r.x, r.y, .0f}; return res; } void COccupancyGridMap2D::nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const + std::vector& out_dists_sqr, + std::optional>& resultIndices) const { // delegate to the 2D version: std::vector r; - nn_multiple_search({query.x, query.y}, N, r, out_dists_sqr); + nn_multiple_search({query.x, query.y}, N, r, out_dists_sqr, resultIndices); results.resize(r.size()); for (size_t i = 0; i < r.size(); i++) results[i] = {r[i].x, r[i].y, .0f}; @@ -793,11 +795,13 @@ void COccupancyGridMap2D::nn_multiple_search( void COccupancyGridMap2D::nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const + std::vector& out_dists_sqr, + std::optional>& resultIndices) const { // delegate to the 2D version: std::vector r; - nn_radius_search({query.x, query.y}, search_radius_sqr, r, out_dists_sqr); + nn_radius_search( + {query.x, query.y}, search_radius_sqr, r, out_dists_sqr, resultIndices); results.resize(r.size()); for (size_t i = 0; i < r.size(); i++) results[i] = {r[i].x, r[i].y, .0f}; @@ -805,11 +809,13 @@ void COccupancyGridMap2D::nn_radius_search( bool COccupancyGridMap2D::nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr) const + float& out_dist_sqr, std::optional& resultIndex) const { std::vector r; std::vector dist_sqr; - nn_multiple_search(query, 1, r, dist_sqr); + std::optional> resultIndices; + resultIndex.reset(); // not supported in gridmaps + nn_multiple_search(query, 1, r, dist_sqr, resultIndices); if (r.empty()) return false; // none found result = r[0]; out_dist_sqr = dist_sqr[0]; @@ -819,7 +825,8 @@ bool COccupancyGridMap2D::nn_single_search( void COccupancyGridMap2D::nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const + std::vector& out_dists_sqr, + std::optional>& resultIndices) const { results.clear(); results.reserve(N); @@ -890,7 +897,8 @@ void COccupancyGridMap2D::nn_multiple_search( void COccupancyGridMap2D::nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const + std::vector& out_dists_sqr, + std::optional>& resultIndices) const { results.clear(); out_dists_sqr.clear(); diff --git a/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp b/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp index a57b69e6a0..b4da771898 100644 --- a/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp +++ b/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp @@ -53,17 +53,22 @@ TEST(COccupancyGridMap2DTests, NearestNeighborsCapable) { mrpt::math::TPoint2Df result; + std::optional resultIndex; float out_dist_sqr = 0; - bool found = nn.nn_single_search({0.90f, 1.95f}, result, out_dist_sqr); + bool found = nn.nn_single_search( + {0.90f, 1.95f}, result, out_dist_sqr, resultIndex); EXPECT_TRUE(found); EXPECT_NEAR(result.x, 1.0f, resolution); EXPECT_NEAR(result.y, 2.0f, resolution); EXPECT_NEAR(std::sqrt(out_dist_sqr), 0.15f, resolution); + EXPECT_EQ(resultIndex.has_value(), nn.nn_supports_indices()); } { mrpt::math::TPoint2Df result; float out_dist_sqr = 0; - bool found = nn.nn_single_search({-4.0f, 2.1f}, result, out_dist_sqr); + std::optional resultIndex; + bool found = nn.nn_single_search( + {-4.0f, 2.1f}, result, out_dist_sqr, resultIndex); EXPECT_TRUE(found); EXPECT_NEAR(result.x, 1.0f, resolution); EXPECT_NEAR(result.y, 2.0f, resolution); @@ -73,7 +78,9 @@ TEST(COccupancyGridMap2DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; - nn.nn_multiple_search({-2.0f, 5.0f}, 2, results, out_dists_sqr); + std::optional> resultIndices; + nn.nn_multiple_search( + {-2.0f, 5.0f}, 2, results, out_dists_sqr, resultIndices); EXPECT_EQ(results.size(), 2UL); EXPECT_EQ(out_dists_sqr.size(), results.size()); @@ -93,8 +100,10 @@ TEST(COccupancyGridMap2DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; + std::optional> resultIndices; nn.nn_radius_search( - {-2.0f, 5.0f}, mrpt::square(10.0f), results, out_dists_sqr); + {-2.0f, 5.0f}, mrpt::square(10.0f), results, out_dists_sqr, + resultIndices); EXPECT_EQ(results.size(), occupiedPoints.size()); EXPECT_EQ(out_dists_sqr.size(), results.size()); @@ -108,8 +117,10 @@ TEST(COccupancyGridMap2DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; + std::optional> resultIndices; nn.nn_radius_search( - {0.9f, 1.9f}, mrpt::square(1.0f), results, out_dists_sqr); + {0.9f, 1.9f}, mrpt::square(1.0f), results, out_dists_sqr, + resultIndices); EXPECT_EQ(results.size(), 3UL); EXPECT_EQ(out_dists_sqr.size(), results.size()); @@ -117,8 +128,10 @@ TEST(COccupancyGridMap2DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; + std::optional> resultIndices; nn.nn_radius_search( - {0.5f, 1.5f}, mrpt::square(0.5f), results, out_dists_sqr); + {0.5f, 1.5f}, mrpt::square(0.5f), results, out_dists_sqr, + resultIndices); EXPECT_EQ(results.size(), 0UL); EXPECT_EQ(out_dists_sqr.size(), results.size()); diff --git a/libs/maps/src/maps/COccupancyGridMap3D.cpp b/libs/maps/src/maps/COccupancyGridMap3D.cpp index 1a578cd222..812e772695 100644 --- a/libs/maps/src/maps/COccupancyGridMap3D.cpp +++ b/libs/maps/src/maps/COccupancyGridMap3D.cpp @@ -493,32 +493,36 @@ void COccupancyGridMap3D::saveMetricMapRepresentationToFile( bool COccupancyGridMap3D::nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr) const + float& out_dist_sqr, std::optional& resultIndex) const { THROW_EXCEPTION("Cannot run a 2D search on a 3D gridmap"); } void COccupancyGridMap3D::nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const + std::vector& out_dists_sqr, + std::optional>& resultIndices) const { THROW_EXCEPTION("Cannot run a 2D search on a 3D gridmap"); } void COccupancyGridMap3D::nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const + std::vector& out_dists_sqr, + std::optional>& resultIndices) const { THROW_EXCEPTION("Cannot run a 2D search on a 3D gridmap"); } bool COccupancyGridMap3D::nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr) const + float& out_dist_sqr, std::optional& resultIndex) const { std::vector r; std::vector dist_sqr; - nn_multiple_search(query, 1, r, dist_sqr); + std::optional> resultIndices; + resultIndex.reset(); // not supported in gridmaps + nn_multiple_search(query, 1, r, dist_sqr, resultIndices); if (r.empty()) return false; // none found result = r[0]; out_dist_sqr = dist_sqr[0]; @@ -528,7 +532,8 @@ bool COccupancyGridMap3D::nn_single_search( void COccupancyGridMap3D::nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const + std::vector& out_dists_sqr, + std::optional>& resultIndices) const { results.clear(); results.reserve(N); @@ -625,7 +630,8 @@ void COccupancyGridMap3D::nn_multiple_search( void COccupancyGridMap3D::nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const + std::vector& out_dists_sqr, + std::optional>& resultIndices) const { results.clear(); out_dists_sqr.clear(); diff --git a/libs/maps/src/maps/COccupancyGridMap3D_unittest.cpp b/libs/maps/src/maps/COccupancyGridMap3D_unittest.cpp index 08bedac437..123ff6d8ec 100644 --- a/libs/maps/src/maps/COccupancyGridMap3D_unittest.cpp +++ b/libs/maps/src/maps/COccupancyGridMap3D_unittest.cpp @@ -53,19 +53,22 @@ TEST(COccupancyGridMap3DTests, NearestNeighborsCapable) { mrpt::math::TPoint3Df result; + std::optional resultIndex; float out_dist_sqr = 0; - bool found = - nn.nn_single_search({0.90f, 1.95f, 1.0f}, result, out_dist_sqr); + bool found = nn.nn_single_search( + {0.90f, 1.95f, 1.0f}, result, out_dist_sqr, resultIndex); EXPECT_TRUE(found); EXPECT_NEAR(result.x, 1.0f, resolution); EXPECT_NEAR(result.y, 2.0f, resolution); EXPECT_NEAR(std::sqrt(out_dist_sqr), 0.15f, resolution); + EXPECT_EQ(resultIndex.has_value(), nn.nn_supports_indices()); } { mrpt::math::TPoint3Df result; + std::optional resultIndex; float out_dist_sqr = 0; - bool found = - nn.nn_single_search({-4.0f, 2.1f, 1.0f}, result, out_dist_sqr); + bool found = nn.nn_single_search( + {-4.0f, 2.1f, 1.0f}, result, out_dist_sqr, resultIndex); EXPECT_TRUE(found); EXPECT_NEAR(result.x, 1.0f, resolution); EXPECT_NEAR(result.y, 2.0f, resolution); @@ -75,10 +78,13 @@ TEST(COccupancyGridMap3DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; - nn.nn_multiple_search({-2.0f, 5.0f, 1.0f}, 2, results, out_dists_sqr); + std::optional> resultIndices; + nn.nn_multiple_search( + {-2.0f, 5.0f, 1.0f}, 2, results, out_dists_sqr, resultIndices); EXPECT_EQ(results.size(), 2UL); EXPECT_EQ(out_dists_sqr.size(), results.size()); + EXPECT_EQ(resultIndices.has_value(), nn.nn_supports_indices()); EXPECT_NEAR(results.at(0).x, 1.0f, resolution); EXPECT_NEAR(results.at(0).y, 2.0f, resolution); @@ -95,8 +101,10 @@ TEST(COccupancyGridMap3DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; + std::optional> resultIndices; nn.nn_radius_search( - {-2.0f, 5.0f, 1.0f}, mrpt::square(10.0f), results, out_dists_sqr); + {-2.0f, 5.0f, 1.0f}, mrpt::square(10.0f), results, out_dists_sqr, + resultIndices); EXPECT_EQ(results.size(), occupiedPoints.size()); EXPECT_EQ(out_dists_sqr.size(), results.size()); @@ -110,8 +118,10 @@ TEST(COccupancyGridMap3DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; + std::optional> resultIndices; nn.nn_radius_search( - {0.9f, 1.9f, 1.0f}, mrpt::square(1.0f), results, out_dists_sqr); + {0.9f, 1.9f, 1.0f}, mrpt::square(1.0f), results, out_dists_sqr, + resultIndices); EXPECT_EQ(results.size(), 3UL); EXPECT_EQ(out_dists_sqr.size(), results.size()); @@ -119,8 +129,10 @@ TEST(COccupancyGridMap3DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; + std::optional> resultIndices; nn.nn_radius_search( - {0.5f, 1.5f, 1.0f}, mrpt::square(0.5f), results, out_dists_sqr); + {0.5f, 1.5f, 1.0f}, mrpt::square(0.5f), results, out_dists_sqr, + resultIndices); EXPECT_EQ(results.size(), 0UL); EXPECT_EQ(out_dists_sqr.size(), results.size()); diff --git a/libs/maps/src/maps/CPointsMap.cpp b/libs/maps/src/maps/CPointsMap.cpp index 8e662503bf..01f99b2f67 100644 --- a/libs/maps/src/maps/CPointsMap.cpp +++ b/libs/maps/src/maps/CPointsMap.cpp @@ -2102,7 +2102,7 @@ void CPointsMap::loadFromVelodyneScan( // See docs in base class bool CPointsMap::nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr) const + float& out_dist_sqr, std::optional& resultIndex) const { try { @@ -2119,7 +2119,7 @@ bool CPointsMap::nn_single_search( } bool CPointsMap::nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr) const + float& out_dist_sqr, std::optional& resultIndex) const { try { @@ -2136,7 +2136,8 @@ bool CPointsMap::nn_single_search( void CPointsMap::nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const + std::vector& out_dists_sqr, + std::optional>& resultIndices) const { std::vector idxs; kdTreeNClosestPoint3DIdx(query.x, query.y, query.z, N, idxs, out_dists_sqr); @@ -2147,7 +2148,8 @@ void CPointsMap::nn_multiple_search( void CPointsMap::nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, - std::vector& out_dists_sqr) const + std::vector& out_dists_sqr, + std::optional>& resultIndices) const { std::vector idxs; kdTreeNClosestPoint2DIdx(query.x, query.y, N, idxs, out_dists_sqr); @@ -2159,7 +2161,8 @@ void CPointsMap::nn_multiple_search( void CPointsMap::nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const + std::vector& out_dists_sqr, + std::optional>& resultIndices) const { std::vector> indices_dist; kdTreeRadiusSearch3D( @@ -2177,7 +2180,8 @@ void CPointsMap::nn_radius_search( void CPointsMap::nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, - std::vector& out_dists_sqr) const + std::vector& out_dists_sqr, + std::optional>& resultIndices) const { std::vector> indices_dist; kdTreeRadiusSearch2D(query.x, query.y, search_radius_sqr, indices_dist); From ec2e05ef89aeaca755a1c2af8ee6d603f746748c Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 5 Nov 2023 20:23:29 +0100 Subject: [PATCH 15/36] Update python wrapper --- python/src/mrpt/maps/CColouredOctoMap.cpp | 21 ++-------- python/src/mrpt/maps/COccupancyGridMap2D.cpp | 24 +++-------- python/src/mrpt/maps/COccupancyGridMap3D.cpp | 24 +++-------- python/src/mrpt/maps/COctoMap.cpp | 21 ++-------- .../mrpt/maps/CPointCloudFilterByDistance.cpp | 21 ++-------- python/src/mrpt/maps/CPointsMap.cpp | 3 +- python/src/mrpt/maps/CPointsMap_1.cpp | 2 +- .../src/mrpt/maps/CReflectivityGridMap2D.cpp | 21 ++-------- python/src/mrpt/maps/CVoxelMap.cpp | 42 ++++--------------- .../mrpt/maps/CVoxelMapOccupancyBase_1.cpp | 6 +-- .../mrpt/maps/CVoxelMapOccupancyBase_2.cpp | 6 +-- .../src/mrpt/maps/NearestNeighborsCapable.cpp | 13 +----- .../COccupancyGridMapFeatureExtractor.cpp | 1 - 13 files changed, 42 insertions(+), 163 deletions(-) diff --git a/python/src/mrpt/maps/CColouredOctoMap.cpp b/python/src/mrpt/maps/CColouredOctoMap.cpp index 6773d3290e..b222891b60 100644 --- a/python/src/mrpt/maps/CColouredOctoMap.cpp +++ b/python/src/mrpt/maps/CColouredOctoMap.cpp @@ -986,31 +986,18 @@ struct PyCallBack_mrpt_maps_CColouredPointsMap : public mrpt::maps::CColouredPoi } return CPointsMap::asString(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + bool nn_supports_indices() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); if (overload) { - auto o = overload.operator()(a0, a1, a2); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); - } - else return pybind11::detail::cast_safe(std::move(o)); - } - return CPointsMap::nn_single_search(a0, a1, a2); - } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { - pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); - if (overload) { - auto o = overload.operator()(a0, a1, a2); + auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { static pybind11::detail::override_caster_t caster; return pybind11::detail::cast_ref(std::move(o), caster); } else return pybind11::detail::cast_safe(std::move(o)); } - return CPointsMap::nn_single_search(a0, a1, a2); + return CPointsMap::nn_supports_indices(); } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; diff --git a/python/src/mrpt/maps/COccupancyGridMap2D.cpp b/python/src/mrpt/maps/COccupancyGridMap2D.cpp index b6bdbc90a7..366eec9843 100644 --- a/python/src/mrpt/maps/COccupancyGridMap2D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap2D.cpp @@ -283,31 +283,18 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap2D : public mrpt::maps::COccupancyG } return COccupancyGridMap2D::asString(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + bool nn_supports_indices() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); if (overload) { - auto o = overload.operator()(a0, a1, a2); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); - } - else return pybind11::detail::cast_safe(std::move(o)); - } - return COccupancyGridMap2D::nn_single_search(a0, a1, a2); - } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { - pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); - if (overload) { - auto o = overload.operator()(a0, a1, a2); + auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { static pybind11::detail::override_caster_t caster; return pybind11::detail::cast_ref(std::move(o), caster); } else return pybind11::detail::cast_safe(std::move(o)); } - return COccupancyGridMap2D::nn_single_search(a0, a1, a2); + return COccupancyGridMap2D::nn_supports_indices(); } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; @@ -609,8 +596,7 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s cl.def("compute3DMatchingRatio", (float (mrpt::maps::COccupancyGridMap2D::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const) &mrpt::maps::COccupancyGridMap2D::compute3DMatchingRatio, "See docs in base class: in this class this always returns 0 \n\nC++: mrpt::maps::COccupancyGridMap2D::compute3DMatchingRatio(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const --> float", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("params")); cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::COccupancyGridMap2D::*)(const std::string &) const) &mrpt::maps::COccupancyGridMap2D::saveMetricMapRepresentationToFile, "This virtual method saves the map to a file \"filNamePrefix\"+<\n some_file_extension >, as an image or in any other applicable way (Notice\n that other methods to save the map may be implemented in classes\n implementing this virtual interface). \n\nC++: mrpt::maps::COccupancyGridMap2D::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("filNamePrefix")); cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap2D::asString() const --> std::string"); - cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, " @{ \n\nC++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); - cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_supports_indices", (bool (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::nn_supports_indices, " @{ \n\nC++: mrpt::maps::COccupancyGridMap2D::nn_supports_indices() const --> bool"); cl.def("assign", (class mrpt::maps::COccupancyGridMap2D & (mrpt::maps::COccupancyGridMap2D::*)(const class mrpt::maps::COccupancyGridMap2D &)) &mrpt::maps::COccupancyGridMap2D::operator=, "C++: mrpt::maps::COccupancyGridMap2D::operator=(const class mrpt::maps::COccupancyGridMap2D &) --> class mrpt::maps::COccupancyGridMap2D &", pybind11::return_value_policy::automatic, pybind11::arg("")); { // mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly file:mrpt/maps/COccupancyGridMap2D.h line:215 diff --git a/python/src/mrpt/maps/COccupancyGridMap3D.cpp b/python/src/mrpt/maps/COccupancyGridMap3D.cpp index e1f5b4bd8a..2edc4846d1 100644 --- a/python/src/mrpt/maps/COccupancyGridMap3D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap3D.cpp @@ -276,31 +276,18 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D : public mrpt::maps::COccupancyG } return COccupancyGridMap3D::asString(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + bool nn_supports_indices() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); if (overload) { - auto o = overload.operator()(a0, a1, a2); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); - } - else return pybind11::detail::cast_safe(std::move(o)); - } - return COccupancyGridMap3D::nn_single_search(a0, a1, a2); - } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { - pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); - if (overload) { - auto o = overload.operator()(a0, a1, a2); + auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { static pybind11::detail::override_caster_t caster; return pybind11::detail::cast_ref(std::move(o), caster); } else return pybind11::detail::cast_safe(std::move(o)); } - return COccupancyGridMap3D::nn_single_search(a0, a1, a2); + return COccupancyGridMap3D::nn_supports_indices(); } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; @@ -513,8 +500,7 @@ void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::s cl.def("compute3DMatchingRatio", (float (mrpt::maps::COccupancyGridMap3D::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const) &mrpt::maps::COccupancyGridMap3D::compute3DMatchingRatio, "See docs in base class: in this class this always returns 0 \n\nC++: mrpt::maps::COccupancyGridMap3D::compute3DMatchingRatio(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const --> float", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("params")); cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::COccupancyGridMap3D::*)(const std::string &) const) &mrpt::maps::COccupancyGridMap3D::saveMetricMapRepresentationToFile, "C++: mrpt::maps::COccupancyGridMap3D::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("f")); cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap3D::asString() const --> std::string"); - cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, " @{ \n\nC++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); - cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_supports_indices", (bool (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::nn_supports_indices, " @{ \n\nC++: mrpt::maps::COccupancyGridMap3D::nn_supports_indices() const --> bool"); cl.def("assign", (class mrpt::maps::COccupancyGridMap3D & (mrpt::maps::COccupancyGridMap3D::*)(const class mrpt::maps::COccupancyGridMap3D &)) &mrpt::maps::COccupancyGridMap3D::operator=, "C++: mrpt::maps::COccupancyGridMap3D::operator=(const class mrpt::maps::COccupancyGridMap3D &) --> class mrpt::maps::COccupancyGridMap3D &", pybind11::return_value_policy::automatic, pybind11::arg("")); { // mrpt::maps::COccupancyGridMap3D::TInsertionOptions file:mrpt/maps/COccupancyGridMap3D.h line:221 diff --git a/python/src/mrpt/maps/COctoMap.cpp b/python/src/mrpt/maps/COctoMap.cpp index a6d75572a7..e02ce2c56a 100644 --- a/python/src/mrpt/maps/COctoMap.cpp +++ b/python/src/mrpt/maps/COctoMap.cpp @@ -991,31 +991,18 @@ struct PyCallBack_mrpt_maps_CSimplePointsMap : public mrpt::maps::CSimplePointsM } return CPointsMap::asString(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + bool nn_supports_indices() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); if (overload) { - auto o = overload.operator()(a0, a1, a2); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); - } - else return pybind11::detail::cast_safe(std::move(o)); - } - return CPointsMap::nn_single_search(a0, a1, a2); - } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { - pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); - if (overload) { - auto o = overload.operator()(a0, a1, a2); + auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { static pybind11::detail::override_caster_t caster; return pybind11::detail::cast_ref(std::move(o), caster); } else return pybind11::detail::cast_safe(std::move(o)); } - return CPointsMap::nn_single_search(a0, a1, a2); + return CPointsMap::nn_supports_indices(); } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; diff --git a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp index fc0d26c3b3..0310045dd6 100644 --- a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp +++ b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp @@ -555,31 +555,18 @@ struct PyCallBack_mrpt_maps_CPointsMapXYZI : public mrpt::maps::CPointsMapXYZI { } return CPointsMap::asString(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + bool nn_supports_indices() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); if (overload) { - auto o = overload.operator()(a0, a1, a2); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); - } - else return pybind11::detail::cast_safe(std::move(o)); - } - return CPointsMap::nn_single_search(a0, a1, a2); - } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { - pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); - if (overload) { - auto o = overload.operator()(a0, a1, a2); + auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { static pybind11::detail::override_caster_t caster; return pybind11::detail::cast_ref(std::move(o), caster); } else return pybind11::detail::cast_safe(std::move(o)); } - return CPointsMap::nn_single_search(a0, a1, a2); + return CPointsMap::nn_supports_indices(); } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; diff --git a/python/src/mrpt/maps/CPointsMap.cpp b/python/src/mrpt/maps/CPointsMap.cpp index 330f66a1ac..9a8902d620 100644 --- a/python/src/mrpt/maps/CPointsMap.cpp +++ b/python/src/mrpt/maps/CPointsMap.cpp @@ -253,8 +253,7 @@ void bind_mrpt_maps_CPointsMap(std::function< pybind11::module &(std::string con cl.def("kdtree_distance", (float (mrpt::maps::CPointsMap::*)(const float *, size_t, size_t) const) &mrpt::maps::CPointsMap::kdtree_distance, "Returns the distance between the vector \"p1[0:size-1]\" and the data\n point with index \"idx_p2\" stored in the class:\n\nC++: mrpt::maps::CPointsMap::kdtree_distance(const float *, size_t, size_t) const --> float", pybind11::arg("p1"), pybind11::arg("idx_p2"), pybind11::arg("size")); cl.def("mark_as_modified", (void (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::mark_as_modified, "Users normally don't need to call this. Called by this class or children\n classes, set m_largestDistanceFromOriginIsUpdated=false, invalidates the\n kd-tree cache, and such. \n\nC++: mrpt::maps::CPointsMap::mark_as_modified() const --> void"); cl.def("asString", (std::string (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::CPointsMap::asString() const --> std::string"); - cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::CPointsMap::nn_single_search, " @{ \n\nC++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); - cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_supports_indices", (bool (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::nn_supports_indices, " @{ \n\nC++: mrpt::maps::CPointsMap::nn_supports_indices() const --> bool"); { // mrpt::maps::CPointsMap::TInsertionOptions file:mrpt/maps/CPointsMap.h line:228 auto & enclosing_class = cl; diff --git a/python/src/mrpt/maps/CPointsMap_1.cpp b/python/src/mrpt/maps/CPointsMap_1.cpp index 2c7b3e91d7..959d787e2a 100644 --- a/python/src/mrpt/maps/CPointsMap_1.cpp +++ b/python/src/mrpt/maps/CPointsMap_1.cpp @@ -39,7 +39,7 @@ void bind_mrpt_maps_CPointsMap_1(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::opengl::PointCloudAdapter file:mrpt/maps/CPointsMap.h line:1239 + { // mrpt::opengl::PointCloudAdapter file:mrpt/maps/CPointsMap.h line:1246 pybind11::class_, std::shared_ptr>> cl(M("mrpt::opengl"), "PointCloudAdapter_mrpt_maps_CPointsMap_t", "Specialization mrpt::opengl::PointCloudAdapter\n \n"); cl.def( pybind11::init(), pybind11::arg("obj") ); diff --git a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp index 1449857c70..3ea5c58cc0 100644 --- a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp +++ b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp @@ -824,31 +824,18 @@ struct PyCallBack_mrpt_maps_CWeightedPointsMap : public mrpt::maps::CWeightedPoi } return CPointsMap::asString(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + bool nn_supports_indices() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); if (overload) { - auto o = overload.operator()(a0, a1, a2); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); - } - else return pybind11::detail::cast_safe(std::move(o)); - } - return CPointsMap::nn_single_search(a0, a1, a2); - } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { - pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); - if (overload) { - auto o = overload.operator()(a0, a1, a2); + auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { static pybind11::detail::override_caster_t caster; return pybind11::detail::cast_ref(std::move(o), caster); } else return pybind11::detail::cast_safe(std::move(o)); } - return CPointsMap::nn_single_search(a0, a1, a2); + return CPointsMap::nn_supports_indices(); } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; diff --git a/python/src/mrpt/maps/CVoxelMap.cpp b/python/src/mrpt/maps/CVoxelMap.cpp index 90fec9256e..563fd2536b 100644 --- a/python/src/mrpt/maps/CVoxelMap.cpp +++ b/python/src/mrpt/maps/CVoxelMap.cpp @@ -212,31 +212,18 @@ struct PyCallBack_mrpt_maps_CVoxelMap : public mrpt::maps::CVoxelMap { } return CVoxelMapOccupancyBase::boundingBox(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + bool nn_supports_indices() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); if (overload) { - auto o = overload.operator()(a0, a1, a2); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); - } - else return pybind11::detail::cast_safe(std::move(o)); - } - return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2); - } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { - pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); - if (overload) { - auto o = overload.operator()(a0, a1, a2); + auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { static pybind11::detail::override_caster_t caster; return pybind11::detail::cast_ref(std::move(o), caster); } else return pybind11::detail::cast_safe(std::move(o)); } - return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2); + return CVoxelMapOccupancyBase::nn_supports_indices(); } void internal_clear() override { pybind11::gil_scoped_acquire gil; @@ -536,31 +523,18 @@ struct PyCallBack_mrpt_maps_CVoxelMapRGB : public mrpt::maps::CVoxelMapRGB { } return CVoxelMapOccupancyBase::boundingBox(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2) const override { + bool nn_supports_indices() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); if (overload) { - auto o = overload.operator()(a0, a1, a2); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); - } - else return pybind11::detail::cast_safe(std::move(o)); - } - return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2); - } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2) const override { - pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); - if (overload) { - auto o = overload.operator()(a0, a1, a2); + auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { static pybind11::detail::override_caster_t caster; return pybind11::detail::cast_ref(std::move(o), caster); } else return pybind11::detail::cast_safe(std::move(o)); } - return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2); + return CVoxelMapOccupancyBase::nn_supports_indices(); } void internal_clear() override { pybind11::gil_scoped_acquire gil; diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp index 45a1bd2307..ac7897c374 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp @@ -67,8 +67,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s cl.def_static("l2p", (float (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p(const signed char) --> float", pybind11::arg("l")); cl.def_static("l2p_255", (uint8_t (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p_255, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p_255(const signed char) --> uint8_t", pybind11::arg("l")); cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); - cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); - cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_supports_indices", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_supports_indices, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_supports_indices() const --> bool"); cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); @@ -99,8 +98,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s cl.def("squareDistanceToClosestCorrespondence", (float (mrpt::maps::CMetricMap::*)(float, float) const) &mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence, "Returns the square distance from the 2D point (x0,y0) to the closest\n correspondence in the map. \n\nC++: mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence(float, float) const --> float", pybind11::arg("x0"), pybind11::arg("y0")); cl.def("getAsSimplePointsMap", (class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMetricMap::getAsSimplePointsMap() --> class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_supports_indices", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_supports_indices, "Returns true if the rest of `nn_*` methods will populate the indices\n std::optional<> return variables, false otherwise. \n\nC++: mrpt::maps::NearestNeighborsCapable::nn_supports_indices() const --> bool"); cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp index 6deee581f7..9c74f77800 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp @@ -67,8 +67,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s cl.def_static("l2p", (float (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p(const signed char) --> float", pybind11::arg("l")); cl.def_static("l2p_255", (uint8_t (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p_255, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p_255(const signed char) --> uint8_t", pybind11::arg("l")); cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); - cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); - cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_supports_indices", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_supports_indices, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_supports_indices() const --> bool"); cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); @@ -99,8 +98,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s cl.def("squareDistanceToClosestCorrespondence", (float (mrpt::maps::CMetricMap::*)(float, float) const) &mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence, "Returns the square distance from the 2D point (x0,y0) to the closest\n correspondence in the map. \n\nC++: mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence(float, float) const --> float", pybind11::arg("x0"), pybind11::arg("y0")); cl.def("getAsSimplePointsMap", (class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMetricMap::getAsSimplePointsMap() --> class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_supports_indices", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_supports_indices, "Returns true if the rest of `nn_*` methods will populate the indices\n std::optional<> return variables, false otherwise. \n\nC++: mrpt::maps::NearestNeighborsCapable::nn_supports_indices() const --> bool"); cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/maps/NearestNeighborsCapable.cpp b/python/src/mrpt/maps/NearestNeighborsCapable.cpp index c62314f21c..f40bea821f 100644 --- a/python/src/mrpt/maps/NearestNeighborsCapable.cpp +++ b/python/src/mrpt/maps/NearestNeighborsCapable.cpp @@ -1,13 +1,5 @@ -#include -#include #include -#include -#include -#include -#include #include // __str__ -#include -#include #include #include @@ -24,10 +16,9 @@ void bind_mrpt_maps_NearestNeighborsCapable(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::NearestNeighborsCapable file:mrpt/maps/NearestNeighborsCapable.h line:26 + { // mrpt::maps::NearestNeighborsCapable file:mrpt/maps/NearestNeighborsCapable.h line:28 pybind11::class_> cl(M("mrpt::maps"), "NearestNeighborsCapable", "Virtual interface for maps having the capability of searching the closest\n neighbor(s) of a given query 2D or 3D point.\n\n Note this is more generic than mrpt::math::KDTreeCapable since it does not\n assume the use of KD-trees, and it is also non templatized, so users can use\n dynamic casting to interact with maps in a generic way.\n\n \n New in MRPT 2.11.3\n \n\n\n "); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr")); + cl.def("nn_supports_indices", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_supports_indices, "Returns true if the rest of `nn_*` methods will populate the indices\n std::optional<> return variables, false otherwise. \n\nC++: mrpt::maps::NearestNeighborsCapable::nn_supports_indices() const --> bool"); cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/slam/COccupancyGridMapFeatureExtractor.cpp b/python/src/mrpt/slam/COccupancyGridMapFeatureExtractor.cpp index bb9779cb57..c9b8abcc00 100644 --- a/python/src/mrpt/slam/COccupancyGridMapFeatureExtractor.cpp +++ b/python/src/mrpt/slam/COccupancyGridMapFeatureExtractor.cpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include From 9a86120387e0ae722cdebe7b1614da6d9126d56c Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 5 Nov 2023 20:53:15 +0100 Subject: [PATCH 16/36] update python stubs --- python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi | 26 +++++++++++----------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi index c1ffc6a3d2..1009cb63e0 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi @@ -1753,7 +1753,7 @@ class COccupancyGridMap2D(CMetricMap, CLogOddsGridMap2D_signed_char_t, NearestNe @overload def loadFromBitmapFile(conststd, float, conststructmrpt) -> bool: ... def loadFromROSMapServerYAML(self, yamlFilePath: str) -> bool: ... - def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + def nn_supports_indices(self) -> bool: ... def p2l(self, *args, **kwargs) -> Any: ... @overload def resizeGrid(self, new_x_min: float, new_x_max: float, new_y_min: float, new_y_max: float) -> None: ... @@ -1977,7 +1977,7 @@ class COccupancyGridMap3D(CMetricMap, CLogOddsGridMap3D_signed_char_t, NearestNe def isEmpty(self, *args, **kwargs) -> Any: ... def l2p(self, *args, **kwargs) -> Any: ... def l2p_255(self, *args, **kwargs) -> Any: ... - def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + def nn_supports_indices(self) -> bool: ... def p2l(self, *args, **kwargs) -> Any: ... @overload def resizeGrid(self, corner_min, corner_max) -> None: ... @@ -2919,7 +2919,7 @@ class CPointsMap(CMetricMap, mrpt.pymrpt.mrpt.opengl.PLY_Importer, mrpt.pymrpt.m def mark_as_modified(self) -> None: ... @overload def mark_as_modified() -> void: ... - def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + def nn_supports_indices(self) -> bool: ... @overload def reserve(self, newLength: int) -> None: ... @overload @@ -3851,13 +3851,13 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t(CVoxelMapBa @overload def loadFromSimpleMap(constclassmrpt) -> void: ... @overload - def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + def nn_supports_indices(self) -> bool: ... @overload - def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + def nn_supports_indices() -> bool: ... @overload - def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + def nn_supports_indices(self) -> bool: ... @overload - def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + def nn_supports_indices() -> bool: ... def p2l(self, *args, **kwargs) -> Any: ... @overload def saveMetricMapRepresentationToFile(self, filNamePrefix: str) -> None: ... @@ -3957,13 +3957,13 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t(CVoxelMa @overload def loadFromSimpleMap(constclassmrpt) -> void: ... @overload - def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + def nn_supports_indices(self) -> bool: ... @overload - def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + def nn_supports_indices() -> bool: ... @overload - def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + def nn_supports_indices(self) -> bool: ... @overload - def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + def nn_supports_indices() -> bool: ... def p2l(self, *args, **kwargs) -> Any: ... @overload def saveMetricMapRepresentationToFile(self, filNamePrefix: str) -> None: ... @@ -4122,9 +4122,9 @@ class NearestNeighborsCapable: def __init__(self, *args, **kwargs) -> None: ... def assign(self) -> NearestNeighborsCapable: ... @overload - def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + def nn_supports_indices(self) -> bool: ... @overload - def nn_single_search(self, query, result, out_dist_sqr: float) -> bool: ... + def nn_supports_indices() -> bool: ... class OccGridCellTraits: def __init__(self) -> None: ... From df98834546cccefacedc5bff79a01c613bf10aee Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 5 Nov 2023 21:49:32 +0100 Subject: [PATCH 17/36] NN API: Add nn_index_count() method --- .../include/mrpt/maps/COccupancyGridMap2D.h | 6 ++-- .../include/mrpt/maps/COccupancyGridMap3D.h | 6 ++-- libs/maps/include/mrpt/maps/CPointsMap.h | 6 ++-- .../mrpt/maps/CVoxelMapOccupancyBase.h | 6 +++- .../mrpt/maps/NearestNeighborsCapable.h | 6 ++++ python/src/mrpt/maps/CColouredOctoMap.cpp | 13 ++++++++ python/src/mrpt/maps/COccupancyGridMap2D.cpp | 14 ++++++++ python/src/mrpt/maps/COccupancyGridMap3D.cpp | 14 ++++++++ python/src/mrpt/maps/COctoMap.cpp | 13 ++++++++ .../mrpt/maps/CPointCloudFilterByDistance.cpp | 13 ++++++++ python/src/mrpt/maps/CPointsMap.cpp | 1 + python/src/mrpt/maps/CPointsMap_1.cpp | 2 +- .../src/mrpt/maps/CReflectivityGridMap2D.cpp | 13 ++++++++ python/src/mrpt/maps/CVoxelMap.cpp | 26 +++++++++++++++ .../mrpt/maps/CVoxelMapOccupancyBase_1.cpp | 2 ++ .../mrpt/maps/CVoxelMapOccupancyBase_2.cpp | 2 ++ .../src/mrpt/maps/NearestNeighborsCapable.cpp | 1 + python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi | 32 +++++++++++++++++++ 18 files changed, 162 insertions(+), 14 deletions(-) diff --git a/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h b/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h index cb672fd855..e42acccdfd 100644 --- a/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h +++ b/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h @@ -1173,10 +1173,8 @@ class COccupancyGridMap2D /** @name API of the NearestNeighborsCapable virtual interface @{ */ // See docs in base class - [[nodiscard]] virtual bool nn_supports_indices() const override - { - return false; - } + [[nodiscard]] bool nn_supports_indices() const override { return false; } + [[nodiscard]] size_t nn_index_count() const override { return 0; } [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, float& out_dist_sqr, std::optional& resultIndex) const override; diff --git a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h index 60dba7f215..f0a8290bfd 100644 --- a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h +++ b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h @@ -400,10 +400,8 @@ class COccupancyGridMap3D /** @name API of the NearestNeighborsCapable virtual interface @{ */ - [[nodiscard]] virtual bool nn_supports_indices() const override - { - return false; - } + [[nodiscard]] bool nn_supports_indices() const override { return false; } + [[nodiscard]] size_t nn_index_count() const override { return 0; } [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, float& out_dist_sqr, std::optional& resultIndex) const override; diff --git a/libs/maps/include/mrpt/maps/CPointsMap.h b/libs/maps/include/mrpt/maps/CPointsMap.h index 4775016c38..22b90f89a9 100644 --- a/libs/maps/include/mrpt/maps/CPointsMap.h +++ b/libs/maps/include/mrpt/maps/CPointsMap.h @@ -1128,10 +1128,8 @@ class CPointsMap : public CMetricMap, /** @name API of the NearestNeighborsCapable virtual interface @{ */ - [[nodiscard]] virtual bool nn_supports_indices() const override - { - return true; - } + [[nodiscard]] bool nn_supports_indices() const override { return true; } + [[nodiscard]] size_t nn_index_count() const override { return size(); } [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, float& out_dist_sqr, std::optional& resultIndex) const override; diff --git a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h index 1b4a6927f9..401d8247ef 100644 --- a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h +++ b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h @@ -292,10 +292,14 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase, /** @name API of the NearestNeighborsCapable virtual interface @{ */ // See docs in base class - [[nodiscard]] virtual bool nn_supports_indices() const override + [[nodiscard]] bool nn_supports_indices() const override { return getOccupiedVoxels()->nn_supports_indices(); } + [[nodiscard]] size_t nn_index_count() const override + { + return getOccupiedVoxels()->nn_index_count(); + } [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, float& out_dist_sqr, std::optional& resultIndex) const override diff --git a/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h b/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h index d69f04c0bd..443b89d357 100644 --- a/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h +++ b/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h @@ -38,6 +38,12 @@ class NearestNeighborsCapable * std::optional<> return variables, false otherwise. */ [[nodiscard]] virtual bool nn_supports_indices() const = 0; + /** If nn_supports_indices() returns `true`, this must return the number of + * "points" (or whatever entity) the indices correspond to. Otherwise, the + * return value should be ignored. + */ + [[nodiscard]] virtual size_t nn_index_count() const = 0; + /** Search for the closest 3D point to a given one. * * \param[in] query The query input point. diff --git a/python/src/mrpt/maps/CColouredOctoMap.cpp b/python/src/mrpt/maps/CColouredOctoMap.cpp index b222891b60..b983cf4af2 100644 --- a/python/src/mrpt/maps/CColouredOctoMap.cpp +++ b/python/src/mrpt/maps/CColouredOctoMap.cpp @@ -999,6 +999,19 @@ struct PyCallBack_mrpt_maps_CColouredPointsMap : public mrpt::maps::CColouredPoi } return CPointsMap::nn_supports_indices(); } + size_t nn_index_count() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_index_count"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_index_count(); + } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "PLY_import_set_face_count"); diff --git a/python/src/mrpt/maps/COccupancyGridMap2D.cpp b/python/src/mrpt/maps/COccupancyGridMap2D.cpp index 366eec9843..bd8344ee20 100644 --- a/python/src/mrpt/maps/COccupancyGridMap2D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap2D.cpp @@ -296,6 +296,19 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap2D : public mrpt::maps::COccupancyG } return COccupancyGridMap2D::nn_supports_indices(); } + size_t nn_index_count() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_index_count"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return COccupancyGridMap2D::nn_index_count(); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); @@ -597,6 +610,7 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::COccupancyGridMap2D::*)(const std::string &) const) &mrpt::maps::COccupancyGridMap2D::saveMetricMapRepresentationToFile, "This virtual method saves the map to a file \"filNamePrefix\"+<\n some_file_extension >, as an image or in any other applicable way (Notice\n that other methods to save the map may be implemented in classes\n implementing this virtual interface). \n\nC++: mrpt::maps::COccupancyGridMap2D::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("filNamePrefix")); cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap2D::asString() const --> std::string"); cl.def("nn_supports_indices", (bool (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::nn_supports_indices, " @{ \n\nC++: mrpt::maps::COccupancyGridMap2D::nn_supports_indices() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::nn_index_count, "C++: mrpt::maps::COccupancyGridMap2D::nn_index_count() const --> size_t"); cl.def("assign", (class mrpt::maps::COccupancyGridMap2D & (mrpt::maps::COccupancyGridMap2D::*)(const class mrpt::maps::COccupancyGridMap2D &)) &mrpt::maps::COccupancyGridMap2D::operator=, "C++: mrpt::maps::COccupancyGridMap2D::operator=(const class mrpt::maps::COccupancyGridMap2D &) --> class mrpt::maps::COccupancyGridMap2D &", pybind11::return_value_policy::automatic, pybind11::arg("")); { // mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly file:mrpt/maps/COccupancyGridMap2D.h line:215 diff --git a/python/src/mrpt/maps/COccupancyGridMap3D.cpp b/python/src/mrpt/maps/COccupancyGridMap3D.cpp index 2edc4846d1..69f40eb75d 100644 --- a/python/src/mrpt/maps/COccupancyGridMap3D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap3D.cpp @@ -289,6 +289,19 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D : public mrpt::maps::COccupancyG } return COccupancyGridMap3D::nn_supports_indices(); } + size_t nn_index_count() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_index_count"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return COccupancyGridMap3D::nn_index_count(); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); @@ -501,6 +514,7 @@ void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::s cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::COccupancyGridMap3D::*)(const std::string &) const) &mrpt::maps::COccupancyGridMap3D::saveMetricMapRepresentationToFile, "C++: mrpt::maps::COccupancyGridMap3D::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("f")); cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap3D::asString() const --> std::string"); cl.def("nn_supports_indices", (bool (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::nn_supports_indices, " @{ \n\nC++: mrpt::maps::COccupancyGridMap3D::nn_supports_indices() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::nn_index_count, "C++: mrpt::maps::COccupancyGridMap3D::nn_index_count() const --> size_t"); cl.def("assign", (class mrpt::maps::COccupancyGridMap3D & (mrpt::maps::COccupancyGridMap3D::*)(const class mrpt::maps::COccupancyGridMap3D &)) &mrpt::maps::COccupancyGridMap3D::operator=, "C++: mrpt::maps::COccupancyGridMap3D::operator=(const class mrpt::maps::COccupancyGridMap3D &) --> class mrpt::maps::COccupancyGridMap3D &", pybind11::return_value_policy::automatic, pybind11::arg("")); { // mrpt::maps::COccupancyGridMap3D::TInsertionOptions file:mrpt/maps/COccupancyGridMap3D.h line:221 diff --git a/python/src/mrpt/maps/COctoMap.cpp b/python/src/mrpt/maps/COctoMap.cpp index e02ce2c56a..9805712856 100644 --- a/python/src/mrpt/maps/COctoMap.cpp +++ b/python/src/mrpt/maps/COctoMap.cpp @@ -1004,6 +1004,19 @@ struct PyCallBack_mrpt_maps_CSimplePointsMap : public mrpt::maps::CSimplePointsM } return CPointsMap::nn_supports_indices(); } + size_t nn_index_count() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_index_count"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_index_count(); + } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "PLY_import_set_face_count"); diff --git a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp index 0310045dd6..fa04eb7363 100644 --- a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp +++ b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp @@ -568,6 +568,19 @@ struct PyCallBack_mrpt_maps_CPointsMapXYZI : public mrpt::maps::CPointsMapXYZI { } return CPointsMap::nn_supports_indices(); } + size_t nn_index_count() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_index_count"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_index_count(); + } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "PLY_import_set_face_count"); diff --git a/python/src/mrpt/maps/CPointsMap.cpp b/python/src/mrpt/maps/CPointsMap.cpp index 9a8902d620..11fb2c5978 100644 --- a/python/src/mrpt/maps/CPointsMap.cpp +++ b/python/src/mrpt/maps/CPointsMap.cpp @@ -254,6 +254,7 @@ void bind_mrpt_maps_CPointsMap(std::function< pybind11::module &(std::string con cl.def("mark_as_modified", (void (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::mark_as_modified, "Users normally don't need to call this. Called by this class or children\n classes, set m_largestDistanceFromOriginIsUpdated=false, invalidates the\n kd-tree cache, and such. \n\nC++: mrpt::maps::CPointsMap::mark_as_modified() const --> void"); cl.def("asString", (std::string (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::CPointsMap::asString() const --> std::string"); cl.def("nn_supports_indices", (bool (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::nn_supports_indices, " @{ \n\nC++: mrpt::maps::CPointsMap::nn_supports_indices() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::nn_index_count, "C++: mrpt::maps::CPointsMap::nn_index_count() const --> size_t"); { // mrpt::maps::CPointsMap::TInsertionOptions file:mrpt/maps/CPointsMap.h line:228 auto & enclosing_class = cl; diff --git a/python/src/mrpt/maps/CPointsMap_1.cpp b/python/src/mrpt/maps/CPointsMap_1.cpp index 959d787e2a..84facc2086 100644 --- a/python/src/mrpt/maps/CPointsMap_1.cpp +++ b/python/src/mrpt/maps/CPointsMap_1.cpp @@ -39,7 +39,7 @@ void bind_mrpt_maps_CPointsMap_1(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::opengl::PointCloudAdapter file:mrpt/maps/CPointsMap.h line:1246 + { // mrpt::opengl::PointCloudAdapter file:mrpt/maps/CPointsMap.h line:1244 pybind11::class_, std::shared_ptr>> cl(M("mrpt::opengl"), "PointCloudAdapter_mrpt_maps_CPointsMap_t", "Specialization mrpt::opengl::PointCloudAdapter\n \n"); cl.def( pybind11::init(), pybind11::arg("obj") ); diff --git a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp index 3ea5c58cc0..9504ee117d 100644 --- a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp +++ b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp @@ -837,6 +837,19 @@ struct PyCallBack_mrpt_maps_CWeightedPointsMap : public mrpt::maps::CWeightedPoi } return CPointsMap::nn_supports_indices(); } + size_t nn_index_count() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_index_count"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_index_count(); + } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "PLY_import_set_face_count"); diff --git a/python/src/mrpt/maps/CVoxelMap.cpp b/python/src/mrpt/maps/CVoxelMap.cpp index 563fd2536b..7a092cd008 100644 --- a/python/src/mrpt/maps/CVoxelMap.cpp +++ b/python/src/mrpt/maps/CVoxelMap.cpp @@ -225,6 +225,19 @@ struct PyCallBack_mrpt_maps_CVoxelMap : public mrpt::maps::CVoxelMap { } return CVoxelMapOccupancyBase::nn_supports_indices(); } + size_t nn_index_count() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_index_count"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::nn_index_count(); + } void internal_clear() override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "internal_clear"); @@ -536,6 +549,19 @@ struct PyCallBack_mrpt_maps_CVoxelMapRGB : public mrpt::maps::CVoxelMapRGB { } return CVoxelMapOccupancyBase::nn_supports_indices(); } + size_t nn_index_count() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_index_count"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::nn_index_count(); + } void internal_clear() override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "internal_clear"); diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp index ac7897c374..e0ce79c4aa 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp @@ -68,6 +68,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s cl.def_static("l2p_255", (uint8_t (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p_255, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p_255(const signed char) --> uint8_t", pybind11::arg("l")); cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); cl.def("nn_supports_indices", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_supports_indices, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_supports_indices() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_index_count, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_index_count() const --> size_t"); cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); @@ -99,6 +100,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s cl.def("getAsSimplePointsMap", (class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMetricMap::getAsSimplePointsMap() --> class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("nn_supports_indices", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_supports_indices, "Returns true if the rest of `nn_*` methods will populate the indices\n std::optional<> return variables, false otherwise. \n\nC++: mrpt::maps::NearestNeighborsCapable::nn_supports_indices() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_supports_indices() returns `true`, this must return the number of\n \"points\" (or whatever entity) the indices correspond to. Otherwise, the\n return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp index 9c74f77800..79e816406a 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp @@ -68,6 +68,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s cl.def_static("l2p_255", (uint8_t (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p_255, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p_255(const signed char) --> uint8_t", pybind11::arg("l")); cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); cl.def("nn_supports_indices", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_supports_indices, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_supports_indices() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_index_count, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_index_count() const --> size_t"); cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); @@ -99,6 +100,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s cl.def("getAsSimplePointsMap", (class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMetricMap::getAsSimplePointsMap() --> class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("nn_supports_indices", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_supports_indices, "Returns true if the rest of `nn_*` methods will populate the indices\n std::optional<> return variables, false otherwise. \n\nC++: mrpt::maps::NearestNeighborsCapable::nn_supports_indices() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_supports_indices() returns `true`, this must return the number of\n \"points\" (or whatever entity) the indices correspond to. Otherwise, the\n return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/maps/NearestNeighborsCapable.cpp b/python/src/mrpt/maps/NearestNeighborsCapable.cpp index f40bea821f..3edb04deb2 100644 --- a/python/src/mrpt/maps/NearestNeighborsCapable.cpp +++ b/python/src/mrpt/maps/NearestNeighborsCapable.cpp @@ -19,6 +19,7 @@ void bind_mrpt_maps_NearestNeighborsCapable(std::function< pybind11::module &(st { // mrpt::maps::NearestNeighborsCapable file:mrpt/maps/NearestNeighborsCapable.h line:28 pybind11::class_> cl(M("mrpt::maps"), "NearestNeighborsCapable", "Virtual interface for maps having the capability of searching the closest\n neighbor(s) of a given query 2D or 3D point.\n\n Note this is more generic than mrpt::math::KDTreeCapable since it does not\n assume the use of KD-trees, and it is also non templatized, so users can use\n dynamic casting to interact with maps in a generic way.\n\n \n New in MRPT 2.11.3\n \n\n\n "); cl.def("nn_supports_indices", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_supports_indices, "Returns true if the rest of `nn_*` methods will populate the indices\n std::optional<> return variables, false otherwise. \n\nC++: mrpt::maps::NearestNeighborsCapable::nn_supports_indices() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_supports_indices() returns `true`, this must return the number of\n \"points\" (or whatever entity) the indices correspond to. Otherwise, the\n return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi index 1009cb63e0..f90d7db557 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi @@ -1753,6 +1753,10 @@ class COccupancyGridMap2D(CMetricMap, CLogOddsGridMap2D_signed_char_t, NearestNe @overload def loadFromBitmapFile(conststd, float, conststructmrpt) -> bool: ... def loadFromROSMapServerYAML(self, yamlFilePath: str) -> bool: ... + @overload + def nn_index_count(self) -> int: ... + @overload + def nn_index_count() -> size_t: ... def nn_supports_indices(self) -> bool: ... def p2l(self, *args, **kwargs) -> Any: ... @overload @@ -1977,6 +1981,10 @@ class COccupancyGridMap3D(CMetricMap, CLogOddsGridMap3D_signed_char_t, NearestNe def isEmpty(self, *args, **kwargs) -> Any: ... def l2p(self, *args, **kwargs) -> Any: ... def l2p_255(self, *args, **kwargs) -> Any: ... + @overload + def nn_index_count(self) -> int: ... + @overload + def nn_index_count() -> size_t: ... def nn_supports_indices(self) -> bool: ... def p2l(self, *args, **kwargs) -> Any: ... @overload @@ -2919,6 +2927,10 @@ class CPointsMap(CMetricMap, mrpt.pymrpt.mrpt.opengl.PLY_Importer, mrpt.pymrpt.m def mark_as_modified(self) -> None: ... @overload def mark_as_modified() -> void: ... + @overload + def nn_index_count(self) -> int: ... + @overload + def nn_index_count() -> size_t: ... def nn_supports_indices(self) -> bool: ... @overload def reserve(self, newLength: int) -> None: ... @@ -3851,6 +3863,14 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t(CVoxelMapBa @overload def loadFromSimpleMap(constclassmrpt) -> void: ... @overload + def nn_index_count(self) -> int: ... + @overload + def nn_index_count() -> size_t: ... + @overload + def nn_index_count(self) -> int: ... + @overload + def nn_index_count() -> size_t: ... + @overload def nn_supports_indices(self) -> bool: ... @overload def nn_supports_indices() -> bool: ... @@ -3957,6 +3977,14 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t(CVoxelMa @overload def loadFromSimpleMap(constclassmrpt) -> void: ... @overload + def nn_index_count(self) -> int: ... + @overload + def nn_index_count() -> size_t: ... + @overload + def nn_index_count(self) -> int: ... + @overload + def nn_index_count() -> size_t: ... + @overload def nn_supports_indices(self) -> bool: ... @overload def nn_supports_indices() -> bool: ... @@ -4122,6 +4150,10 @@ class NearestNeighborsCapable: def __init__(self, *args, **kwargs) -> None: ... def assign(self) -> NearestNeighborsCapable: ... @overload + def nn_index_count(self) -> int: ... + @overload + def nn_index_count() -> size_t: ... + @overload def nn_supports_indices(self) -> bool: ... @overload def nn_supports_indices() -> bool: ... From ab9687c76875f687781a1c79a911ab236aac7b0b Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 5 Nov 2023 23:57:10 +0100 Subject: [PATCH 18/36] NN API: use indices or IDs --- .../include/mrpt/maps/COccupancyGridMap2D.h | 14 ++-- .../include/mrpt/maps/COccupancyGridMap3D.h | 14 ++-- libs/maps/include/mrpt/maps/CPointsMap.h | 14 ++-- .../mrpt/maps/CVoxelMapOccupancyBase.h | 30 +++++---- .../mrpt/maps/NearestNeighborsCapable.h | 42 ++++++------ .../src/maps/COccupancyGridMap2D_common.cpp | 36 +++++++---- .../src/maps/COccupancyGridMap2D_unittest.cpp | 14 ++-- libs/maps/src/maps/COccupancyGridMap3D.cpp | 33 ++++++---- .../src/maps/COccupancyGridMap3D_unittest.cpp | 15 ++--- libs/maps/src/maps/CPointsMap.cpp | 12 ++-- python/src/mrpt/maps/CColouredOctoMap.cpp | 32 +++++++++- python/src/mrpt/maps/COccupancyGridMap2D.cpp | 36 +++++++++-- python/src/mrpt/maps/COccupancyGridMap3D.cpp | 36 +++++++++-- python/src/mrpt/maps/COctoMap.cpp | 32 +++++++++- .../mrpt/maps/CPointCloudFilterByDistance.cpp | 32 +++++++++- python/src/mrpt/maps/CPointsMap.cpp | 4 +- .../src/mrpt/maps/CReflectivityGridMap2D.cpp | 32 +++++++++- python/src/mrpt/maps/CVoxelMap.cpp | 64 +++++++++++++++++-- .../mrpt/maps/CVoxelMapOccupancyBase_1.cpp | 10 ++- .../mrpt/maps/CVoxelMapOccupancyBase_2.cpp | 10 ++- .../src/mrpt/maps/NearestNeighborsCapable.cpp | 16 ++++- .../COccupancyGridMapFeatureExtractor.cpp | 1 + python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi | 58 +++++++++++++---- 23 files changed, 434 insertions(+), 153 deletions(-) diff --git a/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h b/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h index e42acccdfd..b143f5e02a 100644 --- a/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h +++ b/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h @@ -1173,34 +1173,34 @@ class COccupancyGridMap2D /** @name API of the NearestNeighborsCapable virtual interface @{ */ // See docs in base class - [[nodiscard]] bool nn_supports_indices() const override { return false; } + [[nodiscard]] bool nn_has_indices_or_ids() const override { return false; } [[nodiscard]] size_t nn_index_count() const override { return 0; } [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr, std::optional& resultIndex) const override; + float& out_dist_sqr, uint64_t& resultIndexOrID) const override; [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr, std::optional& resultIndex) const override; + float& out_dist_sqr, uint64_t& resultIndexOrID) const override; void nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override; + std::vector& resultIndicesOrIDs) const override; void nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override; + std::vector& resultIndicesOrIDs) const override; void nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override; + std::vector& resultIndicesOrIDs) const override; void nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override; + std::vector& resultIndicesOrIDs) const override; /** @} */ private: diff --git a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h index f0a8290bfd..0d17aaa4e9 100644 --- a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h +++ b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h @@ -400,34 +400,34 @@ class COccupancyGridMap3D /** @name API of the NearestNeighborsCapable virtual interface @{ */ - [[nodiscard]] bool nn_supports_indices() const override { return false; } + [[nodiscard]] bool nn_has_indices_or_ids() const override { return false; } [[nodiscard]] size_t nn_index_count() const override { return 0; } [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr, std::optional& resultIndex) const override; + float& out_dist_sqr, uint64_t& resultIndexOrID) const override; [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr, std::optional& resultIndex) const override; + float& out_dist_sqr, uint64_t& resultIndexOrID) const override; void nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override; + std::vector& resultIndicesOrIDs) const override; void nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override; + std::vector& resultIndicesOrIDs) const override; void nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override; + std::vector& resultIndicesOrIDs) const override; void nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override; + std::vector& resultIndicesOrIDs) const override; /** @} */ private: diff --git a/libs/maps/include/mrpt/maps/CPointsMap.h b/libs/maps/include/mrpt/maps/CPointsMap.h index 22b90f89a9..62d0f17aca 100644 --- a/libs/maps/include/mrpt/maps/CPointsMap.h +++ b/libs/maps/include/mrpt/maps/CPointsMap.h @@ -1128,34 +1128,34 @@ class CPointsMap : public CMetricMap, /** @name API of the NearestNeighborsCapable virtual interface @{ */ - [[nodiscard]] bool nn_supports_indices() const override { return true; } + [[nodiscard]] bool nn_has_indices_or_ids() const override { return true; } [[nodiscard]] size_t nn_index_count() const override { return size(); } [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr, std::optional& resultIndex) const override; + float& out_dist_sqr, uint64_t& resultIndexOrID) const override; [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr, std::optional& resultIndex) const override; + float& out_dist_sqr, uint64_t& resultIndexOrID) const override; void nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override; + std::vector& resultIndicesOrIDs) const override; void nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override; + std::vector& resultIndicesOrIDs) const override; void nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override; + std::vector& resultIndicesOrIDs) const override; void nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override; + std::vector& resultIndicesOrIDs) const override; /** @} */ protected: diff --git a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h index 401d8247ef..3d7201a421 100644 --- a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h +++ b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h @@ -292,9 +292,9 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase, /** @name API of the NearestNeighborsCapable virtual interface @{ */ // See docs in base class - [[nodiscard]] bool nn_supports_indices() const override + [[nodiscard]] bool nn_has_indices_or_ids() const override { - return getOccupiedVoxels()->nn_supports_indices(); + return getOccupiedVoxels()->nn_has_indices_or_ids(); } [[nodiscard]] size_t nn_index_count() const override { @@ -302,53 +302,55 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase, } [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr, std::optional& resultIndex) const override + float& out_dist_sqr, uint64_t& resultIndexOrID) const override { return getOccupiedVoxels()->nn_single_search( - query, result, out_dist_sqr, resultIndex); + query, result, out_dist_sqr, resultIndexOrID); } [[nodiscard]] bool nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr, std::optional& resultIndex) const override + float& out_dist_sqr, uint64_t& resultIndexOrID) const override { return getOccupiedVoxels()->nn_single_search( - query, result, out_dist_sqr, resultIndex); + query, result, out_dist_sqr, resultIndexOrID); } void nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override + std::vector& resultIndicesOrIDs) const override { getOccupiedVoxels()->nn_multiple_search( - query, N, results, out_dists_sqr, resultIndices); + query, N, results, out_dists_sqr, resultIndicesOrIDs); } void nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override + std::vector& resultIndicesOrIDs) const override { getOccupiedVoxels()->nn_multiple_search( - query, N, results, out_dists_sqr, resultIndices); + query, N, results, out_dists_sqr, resultIndicesOrIDs); } void nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override + std::vector& resultIndicesOrIDs) const override { getOccupiedVoxels()->nn_radius_search( - query, search_radius_sqr, results, out_dists_sqr, resultIndices); + query, search_radius_sqr, results, out_dists_sqr, + resultIndicesOrIDs); } void nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const override + std::vector& resultIndicesOrIDs) const override { getOccupiedVoxels()->nn_radius_search( - query, search_radius_sqr, results, out_dists_sqr, resultIndices); + query, search_radius_sqr, results, out_dists_sqr, + resultIndicesOrIDs); } /** @} */ diff --git a/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h b/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h index 443b89d357..2023d04eb1 100644 --- a/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h +++ b/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h @@ -11,8 +11,6 @@ #include #include -#include - namespace mrpt::maps { /** Virtual interface for maps having the capability of searching the closest @@ -34,13 +32,16 @@ class NearestNeighborsCapable /** @name API of the NearestNeighborsCapable virtual interface @{ */ - /** Returns true if the rest of `nn_*` methods will populate the indices - * std::optional<> return variables, false otherwise. */ - [[nodiscard]] virtual bool nn_supports_indices() const = 0; + /** Returns true if the rest of `nn_*` methods will populate the output + * indices values with 0-based contiguous **indices**. + * Returns false if indices are actually sparse **ID numbers** without any + * expectation of they be contiguous or start near zero. + */ + [[nodiscard]] virtual bool nn_has_indices_or_ids() const = 0; - /** If nn_supports_indices() returns `true`, this must return the number of - * "points" (or whatever entity) the indices correspond to. Otherwise, the - * return value should be ignored. + /** If nn_has_indices_or_ids() returns `true`, this must return the number + * of "points" (or whatever entity) the indices correspond to. Otherwise, + * the return value should be ignored. */ [[nodiscard]] virtual size_t nn_index_count() const = 0; @@ -50,20 +51,19 @@ class NearestNeighborsCapable * \param[out] result The found closest point. * \param[out] out_dist_sqr The square Euclidean distance between the query * and the returned point. - * \param[out] resultIndex Depending on the implementation, here it will be - * returned the index of the result point in the map, or std::nullopt if - * indices are not available. + * \param[out] resultIndexOrID The index or ID of the result point in the + * map. * * \return True if successful, false if no point was found. */ [[nodiscard]] virtual bool nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr, std::optional& resultIndex) const = 0; + float& out_dist_sqr, uint64_t& resultIndexOrIDOrID) const = 0; /// \overload for 2D points [[nodiscard]] virtual bool nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr, std::optional& resultIndex) const = 0; + float& out_dist_sqr, uint64_t& resultIndexOrIDOrID) const = 0; /** Search for the `N` closest 3D points to a given one. * @@ -71,23 +71,21 @@ class NearestNeighborsCapable * \param[out] results The found closest points. * \param[out] out_dists_sqr The square Euclidean distances between the * query and the returned point. - * \param[out] resultIndices Depending on the implementation, here it will - * be returned the indices of the result points in the map, or std::nullopt - * if indices are not available. + * \param[out] resultIndicesOrIDs The indices or IDs of the result points. * */ virtual void nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const = 0; + std::vector& resultIndicesOrIDs) const = 0; /// \overload for 2D points virtual void nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const = 0; + std::vector& resultIndicesOrIDs) const = 0; /** Radius search for closest 3D points to a given one. * @@ -96,22 +94,20 @@ class NearestNeighborsCapable * \param[out] results The found closest points. * \param[out] out_dists_sqr The square Euclidean distances between the * query and the returned point. - * \param[out] resultIndices Depending on the implementation, here it will - * be returned the indices of the result points in the map, or std::nullopt - * if indices are not available. + * \param[out] resultIndicesOrIDs The indices or IDs of the result points. */ virtual void nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const = 0; + std::vector& resultIndicesOrIDs) const = 0; /// \overload for 2D points virtual void nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const = 0; + std::vector& resultIndicesOrIDs) const = 0; /** @} */ }; diff --git a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp index 77bf913f95..185358bc0f 100644 --- a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp +++ b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp @@ -770,12 +770,12 @@ float COccupancyGridMap2D::compute3DMatchingRatio( bool COccupancyGridMap2D::nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr, std::optional& resultIndex) const + float& out_dist_sqr, uint64_t& resultIndexOrID) const { // delegate to the 2D version: mrpt::math::TPoint2Df r; bool res = - nn_single_search({query.x, query.y}, r, out_dist_sqr, resultIndex); + nn_single_search({query.x, query.y}, r, out_dist_sqr, resultIndexOrID); result = {r.x, r.y, .0f}; return res; } @@ -783,11 +783,12 @@ void COccupancyGridMap2D::nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const + std::vector& resultIndicesOrIDs) const { // delegate to the 2D version: std::vector r; - nn_multiple_search({query.x, query.y}, N, r, out_dists_sqr, resultIndices); + nn_multiple_search( + {query.x, query.y}, N, r, out_dists_sqr, resultIndicesOrIDs); results.resize(r.size()); for (size_t i = 0; i < r.size(); i++) results[i] = {r[i].x, r[i].y, .0f}; @@ -796,12 +797,13 @@ void COccupancyGridMap2D::nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const + std::vector& resultIndicesOrIDs) const { // delegate to the 2D version: std::vector r; nn_radius_search( - {query.x, query.y}, search_radius_sqr, r, out_dists_sqr, resultIndices); + {query.x, query.y}, search_radius_sqr, r, out_dists_sqr, + resultIndicesOrIDs); results.resize(r.size()); for (size_t i = 0; i < r.size(); i++) results[i] = {r[i].x, r[i].y, .0f}; @@ -809,16 +811,16 @@ void COccupancyGridMap2D::nn_radius_search( bool COccupancyGridMap2D::nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr, std::optional& resultIndex) const + float& out_dist_sqr, uint64_t& resultIndexOrID) const { std::vector r; std::vector dist_sqr; - std::optional> resultIndices; - resultIndex.reset(); // not supported in gridmaps + std::vector resultIndices; nn_multiple_search(query, 1, r, dist_sqr, resultIndices); if (r.empty()) return false; // none found result = r[0]; out_dist_sqr = dist_sqr[0]; + resultIndexOrID = resultIndices[0]; return true; } @@ -826,12 +828,14 @@ void COccupancyGridMap2D::nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const + std::vector& resultIndicesOrIDs) const { results.clear(); results.reserve(N); out_dists_sqr.clear(); out_dists_sqr.reserve(N); + resultIndicesOrIDs.clear(); + resultIndicesOrIDs.reserve(N); int cx_query = x2idx(query.x), cy_query = y2idx(query.y); @@ -887,9 +891,11 @@ void COccupancyGridMap2D::nn_multiple_search( for (auto it = dists2cells.begin(); it != dists2cells.end() && results.size() < N; ++it) { + const int cx = it->second.first; + const int cy = it->second.second; out_dists_sqr.push_back(it->first * resolutionSqr); - results.push_back( - {idx2x(it->second.first), idx2y(it->second.second)}); + results.push_back({idx2x(cx), idx2y(cy)}); + resultIndicesOrIDs.push_back(cx + cy * m_size_x); } } } @@ -898,10 +904,11 @@ void COccupancyGridMap2D::nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const + std::vector& resultIndicesOrIDs) const { results.clear(); out_dists_sqr.clear(); + resultIndicesOrIDs.clear(); if (search_radius_sqr == 0) return; @@ -931,13 +938,14 @@ void COccupancyGridMap2D::nn_radius_search( auto lambdaAddCell = [maxSearchRadiusSqrInCells, cx_query, cy_query, &out_dists_sqr, &results, resolutionSqr, - this](int cx, int cy) { + &resultIndicesOrIDs, this](int cx, int cy) { int distSqr = mrpt::square(cx - cx_query) + mrpt::square(cy - cy_query); if (distSqr > maxSearchRadiusSqrInCells) return; out_dists_sqr.push_back(distSqr * resolutionSqr); results.push_back({idx2x(cx), idx2y(cy)}); + resultIndicesOrIDs.push_back(cx + cy * m_size_x); }; for (int cx = cx0; cx <= cx1; cx++) diff --git a/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp b/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp index b4da771898..4840ebbfe2 100644 --- a/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp +++ b/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp @@ -53,7 +53,7 @@ TEST(COccupancyGridMap2DTests, NearestNeighborsCapable) { mrpt::math::TPoint2Df result; - std::optional resultIndex; + uint64_t resultIndex; float out_dist_sqr = 0; bool found = nn.nn_single_search( {0.90f, 1.95f}, result, out_dist_sqr, resultIndex); @@ -61,12 +61,11 @@ TEST(COccupancyGridMap2DTests, NearestNeighborsCapable) EXPECT_NEAR(result.x, 1.0f, resolution); EXPECT_NEAR(result.y, 2.0f, resolution); EXPECT_NEAR(std::sqrt(out_dist_sqr), 0.15f, resolution); - EXPECT_EQ(resultIndex.has_value(), nn.nn_supports_indices()); } { mrpt::math::TPoint2Df result; float out_dist_sqr = 0; - std::optional resultIndex; + uint64_t resultIndex; bool found = nn.nn_single_search( {-4.0f, 2.1f}, result, out_dist_sqr, resultIndex); EXPECT_TRUE(found); @@ -78,12 +77,13 @@ TEST(COccupancyGridMap2DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; - std::optional> resultIndices; + std::vector resultIndices; nn.nn_multiple_search( {-2.0f, 5.0f}, 2, results, out_dists_sqr, resultIndices); EXPECT_EQ(results.size(), 2UL); EXPECT_EQ(out_dists_sqr.size(), results.size()); + EXPECT_EQ(resultIndices.size(), results.size()); EXPECT_NEAR(results.at(0).x, 1.0f, resolution); EXPECT_NEAR(results.at(0).y, 2.0f, resolution); @@ -100,7 +100,7 @@ TEST(COccupancyGridMap2DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; - std::optional> resultIndices; + std::vector resultIndices; nn.nn_radius_search( {-2.0f, 5.0f}, mrpt::square(10.0f), results, out_dists_sqr, resultIndices); @@ -117,7 +117,7 @@ TEST(COccupancyGridMap2DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; - std::optional> resultIndices; + std::vector resultIndices; nn.nn_radius_search( {0.9f, 1.9f}, mrpt::square(1.0f), results, out_dists_sqr, resultIndices); @@ -128,7 +128,7 @@ TEST(COccupancyGridMap2DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; - std::optional> resultIndices; + std::vector resultIndices; nn.nn_radius_search( {0.5f, 1.5f}, mrpt::square(0.5f), results, out_dists_sqr, resultIndices); diff --git a/libs/maps/src/maps/COccupancyGridMap3D.cpp b/libs/maps/src/maps/COccupancyGridMap3D.cpp index 812e772695..88af2bced9 100644 --- a/libs/maps/src/maps/COccupancyGridMap3D.cpp +++ b/libs/maps/src/maps/COccupancyGridMap3D.cpp @@ -493,7 +493,7 @@ void COccupancyGridMap3D::saveMetricMapRepresentationToFile( bool COccupancyGridMap3D::nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr, std::optional& resultIndex) const + float& out_dist_sqr, uint64_t& resultIndexOrID) const { THROW_EXCEPTION("Cannot run a 2D search on a 3D gridmap"); } @@ -501,7 +501,7 @@ void COccupancyGridMap3D::nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const + std::vector& resultIndicesOrIDs) const { THROW_EXCEPTION("Cannot run a 2D search on a 3D gridmap"); } @@ -509,23 +509,23 @@ void COccupancyGridMap3D::nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const + std::vector& resultIndicesOrIDs) const { THROW_EXCEPTION("Cannot run a 2D search on a 3D gridmap"); } bool COccupancyGridMap3D::nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr, std::optional& resultIndex) const + float& out_dist_sqr, uint64_t& resultIndexOrID) const { std::vector r; std::vector dist_sqr; - std::optional> resultIndices; - resultIndex.reset(); // not supported in gridmaps + std::vector resultIndices; nn_multiple_search(query, 1, r, dist_sqr, resultIndices); if (r.empty()) return false; // none found result = r[0]; out_dist_sqr = dist_sqr[0]; + resultIndexOrID = resultIndices[0]; return true; } @@ -533,12 +533,14 @@ void COccupancyGridMap3D::nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const + std::vector& resultIndicesOrIDs) const { results.clear(); results.reserve(N); out_dists_sqr.clear(); out_dists_sqr.reserve(N); + resultIndicesOrIDs.clear(); + resultIndicesOrIDs.reserve(N); int cx_query = m_grid.x2idx(query.x), cy_query = m_grid.y2idx(query.y), cz_query = m_grid.z2idx(query.z); @@ -619,10 +621,15 @@ void COccupancyGridMap3D::nn_multiple_search( for (auto it = dists2cells.begin(); it != dists2cells.end() && results.size() < N; ++it) { + const int cx = it->second[0]; + const int cy = it->second[1]; + const int cz = it->second[2]; + out_dists_sqr.push_back(it->first * resolutionSqr); results.push_back(mrpt::math::TPoint3Df( - m_grid.idx2x(it->second[0]), m_grid.idx2y(it->second[1]), - m_grid.idx2z(it->second[2]))); + m_grid.idx2x(cx), m_grid.idx2y(cy), m_grid.idx2z(cz))); + resultIndicesOrIDs.push_back( + m_grid.cellAbsIndexFromCXCYCZ(cx, cy, cz)); } } } @@ -631,10 +638,11 @@ void COccupancyGridMap3D::nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const + std::vector& resultIndicesOrIDs) const { results.clear(); out_dists_sqr.clear(); + resultIndicesOrIDs.clear(); if (search_radius_sqr == 0) return; @@ -678,7 +686,8 @@ void COccupancyGridMap3D::nn_radius_search( mrpt::saturate(cz1, 0, sizeZ - 1); auto lambdaAddCell = [maxSearchRadiusSqrInCells, cx_query, cy_query, - cz_query, &out_dists_sqr, &results, resolutionSqr, + cz_query, &out_dists_sqr, &resultIndicesOrIDs, + &results, resolutionSqr, this](int cx, int cy, int cz) { int distSqr = mrpt::square(cx - cx_query) + mrpt::square(cy - cy_query) + mrpt::square(cz - cz_query); @@ -687,6 +696,8 @@ void COccupancyGridMap3D::nn_radius_search( out_dists_sqr.push_back(distSqr * resolutionSqr); results.emplace_back( m_grid.idx2x(cx), m_grid.idx2y(cy), m_grid.idx2z(cz)); + resultIndicesOrIDs.push_back( + m_grid.cellAbsIndexFromCXCYCZ(cx, cy, cz)); }; for (int cx = cx0; cx <= cx1; cx++) diff --git a/libs/maps/src/maps/COccupancyGridMap3D_unittest.cpp b/libs/maps/src/maps/COccupancyGridMap3D_unittest.cpp index 123ff6d8ec..f47bb106c7 100644 --- a/libs/maps/src/maps/COccupancyGridMap3D_unittest.cpp +++ b/libs/maps/src/maps/COccupancyGridMap3D_unittest.cpp @@ -53,7 +53,7 @@ TEST(COccupancyGridMap3DTests, NearestNeighborsCapable) { mrpt::math::TPoint3Df result; - std::optional resultIndex; + uint64_t resultIndex; float out_dist_sqr = 0; bool found = nn.nn_single_search( {0.90f, 1.95f, 1.0f}, result, out_dist_sqr, resultIndex); @@ -61,11 +61,10 @@ TEST(COccupancyGridMap3DTests, NearestNeighborsCapable) EXPECT_NEAR(result.x, 1.0f, resolution); EXPECT_NEAR(result.y, 2.0f, resolution); EXPECT_NEAR(std::sqrt(out_dist_sqr), 0.15f, resolution); - EXPECT_EQ(resultIndex.has_value(), nn.nn_supports_indices()); } { mrpt::math::TPoint3Df result; - std::optional resultIndex; + uint64_t resultIndex; float out_dist_sqr = 0; bool found = nn.nn_single_search( {-4.0f, 2.1f, 1.0f}, result, out_dist_sqr, resultIndex); @@ -78,13 +77,13 @@ TEST(COccupancyGridMap3DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; - std::optional> resultIndices; + std::vector resultIndices; nn.nn_multiple_search( {-2.0f, 5.0f, 1.0f}, 2, results, out_dists_sqr, resultIndices); EXPECT_EQ(results.size(), 2UL); EXPECT_EQ(out_dists_sqr.size(), results.size()); - EXPECT_EQ(resultIndices.has_value(), nn.nn_supports_indices()); + EXPECT_EQ(resultIndices.size(), results.size()); EXPECT_NEAR(results.at(0).x, 1.0f, resolution); EXPECT_NEAR(results.at(0).y, 2.0f, resolution); @@ -101,7 +100,7 @@ TEST(COccupancyGridMap3DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; - std::optional> resultIndices; + std::vector resultIndices; nn.nn_radius_search( {-2.0f, 5.0f, 1.0f}, mrpt::square(10.0f), results, out_dists_sqr, resultIndices); @@ -118,7 +117,7 @@ TEST(COccupancyGridMap3DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; - std::optional> resultIndices; + std::vector resultIndices; nn.nn_radius_search( {0.9f, 1.9f, 1.0f}, mrpt::square(1.0f), results, out_dists_sqr, resultIndices); @@ -129,7 +128,7 @@ TEST(COccupancyGridMap3DTests, NearestNeighborsCapable) { std::vector results; std::vector out_dists_sqr; - std::optional> resultIndices; + std::vector resultIndices; nn.nn_radius_search( {0.5f, 1.5f, 1.0f}, mrpt::square(0.5f), results, out_dists_sqr, resultIndices); diff --git a/libs/maps/src/maps/CPointsMap.cpp b/libs/maps/src/maps/CPointsMap.cpp index 01f99b2f67..048de1e6da 100644 --- a/libs/maps/src/maps/CPointsMap.cpp +++ b/libs/maps/src/maps/CPointsMap.cpp @@ -2102,7 +2102,7 @@ void CPointsMap::loadFromVelodyneScan( // See docs in base class bool CPointsMap::nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, - float& out_dist_sqr, std::optional& resultIndex) const + float& out_dist_sqr, uint64_t& resultIndexOrID) const { try { @@ -2119,7 +2119,7 @@ bool CPointsMap::nn_single_search( } bool CPointsMap::nn_single_search( const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result, - float& out_dist_sqr, std::optional& resultIndex) const + float& out_dist_sqr, uint64_t& resultIndexOrID) const { try { @@ -2137,7 +2137,7 @@ void CPointsMap::nn_multiple_search( const mrpt::math::TPoint3Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const + std::vector& resultIndicesOrIDs) const { std::vector idxs; kdTreeNClosestPoint3DIdx(query.x, query.y, query.z, N, idxs, out_dists_sqr); @@ -2149,7 +2149,7 @@ void CPointsMap::nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const + std::vector& resultIndicesOrIDs) const { std::vector idxs; kdTreeNClosestPoint2DIdx(query.x, query.y, N, idxs, out_dists_sqr); @@ -2162,7 +2162,7 @@ void CPointsMap::nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const + std::vector& resultIndicesOrIDs) const { std::vector> indices_dist; kdTreeRadiusSearch3D( @@ -2181,7 +2181,7 @@ void CPointsMap::nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::optional>& resultIndices) const + std::vector& resultIndicesOrIDs) const { std::vector> indices_dist; kdTreeRadiusSearch2D(query.x, query.y, search_radius_sqr, indices_dist); diff --git a/python/src/mrpt/maps/CColouredOctoMap.cpp b/python/src/mrpt/maps/CColouredOctoMap.cpp index b983cf4af2..64fcaf132e 100644 --- a/python/src/mrpt/maps/CColouredOctoMap.cpp +++ b/python/src/mrpt/maps/CColouredOctoMap.cpp @@ -986,9 +986,9 @@ struct PyCallBack_mrpt_maps_CColouredPointsMap : public mrpt::maps::CColouredPoi } return CPointsMap::asString(); } - bool nn_supports_indices() const override { + bool nn_has_indices_or_ids() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_has_indices_or_ids"); if (overload) { auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { @@ -997,7 +997,7 @@ struct PyCallBack_mrpt_maps_CColouredPointsMap : public mrpt::maps::CColouredPoi } else return pybind11::detail::cast_safe(std::move(o)); } - return CPointsMap::nn_supports_indices(); + return CPointsMap::nn_has_indices_or_ids(); } size_t nn_index_count() const override { pybind11::gil_scoped_acquire gil; @@ -1012,6 +1012,32 @@ struct PyCallBack_mrpt_maps_CColouredPointsMap : public mrpt::maps::CColouredPoi } return CPointsMap::nn_index_count(); } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2, a3); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2, a3); + } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "PLY_import_set_face_count"); diff --git a/python/src/mrpt/maps/COccupancyGridMap2D.cpp b/python/src/mrpt/maps/COccupancyGridMap2D.cpp index bd8344ee20..f9ff15c651 100644 --- a/python/src/mrpt/maps/COccupancyGridMap2D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap2D.cpp @@ -283,9 +283,9 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap2D : public mrpt::maps::COccupancyG } return COccupancyGridMap2D::asString(); } - bool nn_supports_indices() const override { + bool nn_has_indices_or_ids() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_has_indices_or_ids"); if (overload) { auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { @@ -294,7 +294,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap2D : public mrpt::maps::COccupancyG } else return pybind11::detail::cast_safe(std::move(o)); } - return COccupancyGridMap2D::nn_supports_indices(); + return COccupancyGridMap2D::nn_has_indices_or_ids(); } size_t nn_index_count() const override { pybind11::gil_scoped_acquire gil; @@ -309,6 +309,32 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap2D : public mrpt::maps::COccupancyG } return COccupancyGridMap2D::nn_index_count(); } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return COccupancyGridMap2D::nn_single_search(a0, a1, a2, a3); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return COccupancyGridMap2D::nn_single_search(a0, a1, a2, a3); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); @@ -609,8 +635,10 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s cl.def("compute3DMatchingRatio", (float (mrpt::maps::COccupancyGridMap2D::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const) &mrpt::maps::COccupancyGridMap2D::compute3DMatchingRatio, "See docs in base class: in this class this always returns 0 \n\nC++: mrpt::maps::COccupancyGridMap2D::compute3DMatchingRatio(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const --> float", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("params")); cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::COccupancyGridMap2D::*)(const std::string &) const) &mrpt::maps::COccupancyGridMap2D::saveMetricMapRepresentationToFile, "This virtual method saves the map to a file \"filNamePrefix\"+<\n some_file_extension >, as an image or in any other applicable way (Notice\n that other methods to save the map may be implemented in classes\n implementing this virtual interface). \n\nC++: mrpt::maps::COccupancyGridMap2D::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("filNamePrefix")); cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap2D::asString() const --> std::string"); - cl.def("nn_supports_indices", (bool (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::nn_supports_indices, " @{ \n\nC++: mrpt::maps::COccupancyGridMap2D::nn_supports_indices() const --> bool"); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::nn_has_indices_or_ids, " @{ \n\nC++: mrpt::maps::COccupancyGridMap2D::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::nn_index_count, "C++: mrpt::maps::COccupancyGridMap2D::nn_index_count() const --> size_t"); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); cl.def("assign", (class mrpt::maps::COccupancyGridMap2D & (mrpt::maps::COccupancyGridMap2D::*)(const class mrpt::maps::COccupancyGridMap2D &)) &mrpt::maps::COccupancyGridMap2D::operator=, "C++: mrpt::maps::COccupancyGridMap2D::operator=(const class mrpt::maps::COccupancyGridMap2D &) --> class mrpt::maps::COccupancyGridMap2D &", pybind11::return_value_policy::automatic, pybind11::arg("")); { // mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly file:mrpt/maps/COccupancyGridMap2D.h line:215 diff --git a/python/src/mrpt/maps/COccupancyGridMap3D.cpp b/python/src/mrpt/maps/COccupancyGridMap3D.cpp index 69f40eb75d..579c80a3c5 100644 --- a/python/src/mrpt/maps/COccupancyGridMap3D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap3D.cpp @@ -276,9 +276,9 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D : public mrpt::maps::COccupancyG } return COccupancyGridMap3D::asString(); } - bool nn_supports_indices() const override { + bool nn_has_indices_or_ids() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_has_indices_or_ids"); if (overload) { auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { @@ -287,7 +287,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D : public mrpt::maps::COccupancyG } else return pybind11::detail::cast_safe(std::move(o)); } - return COccupancyGridMap3D::nn_supports_indices(); + return COccupancyGridMap3D::nn_has_indices_or_ids(); } size_t nn_index_count() const override { pybind11::gil_scoped_acquire gil; @@ -302,6 +302,32 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D : public mrpt::maps::COccupancyG } return COccupancyGridMap3D::nn_index_count(); } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return COccupancyGridMap3D::nn_single_search(a0, a1, a2, a3); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return COccupancyGridMap3D::nn_single_search(a0, a1, a2, a3); + } bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); @@ -513,8 +539,10 @@ void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::s cl.def("compute3DMatchingRatio", (float (mrpt::maps::COccupancyGridMap3D::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const) &mrpt::maps::COccupancyGridMap3D::compute3DMatchingRatio, "See docs in base class: in this class this always returns 0 \n\nC++: mrpt::maps::COccupancyGridMap3D::compute3DMatchingRatio(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const --> float", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("params")); cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::COccupancyGridMap3D::*)(const std::string &) const) &mrpt::maps::COccupancyGridMap3D::saveMetricMapRepresentationToFile, "C++: mrpt::maps::COccupancyGridMap3D::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("f")); cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap3D::asString() const --> std::string"); - cl.def("nn_supports_indices", (bool (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::nn_supports_indices, " @{ \n\nC++: mrpt::maps::COccupancyGridMap3D::nn_supports_indices() const --> bool"); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::nn_has_indices_or_ids, " @{ \n\nC++: mrpt::maps::COccupancyGridMap3D::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::nn_index_count, "C++: mrpt::maps::COccupancyGridMap3D::nn_index_count() const --> size_t"); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); cl.def("assign", (class mrpt::maps::COccupancyGridMap3D & (mrpt::maps::COccupancyGridMap3D::*)(const class mrpt::maps::COccupancyGridMap3D &)) &mrpt::maps::COccupancyGridMap3D::operator=, "C++: mrpt::maps::COccupancyGridMap3D::operator=(const class mrpt::maps::COccupancyGridMap3D &) --> class mrpt::maps::COccupancyGridMap3D &", pybind11::return_value_policy::automatic, pybind11::arg("")); { // mrpt::maps::COccupancyGridMap3D::TInsertionOptions file:mrpt/maps/COccupancyGridMap3D.h line:221 diff --git a/python/src/mrpt/maps/COctoMap.cpp b/python/src/mrpt/maps/COctoMap.cpp index 9805712856..aaf6de25a4 100644 --- a/python/src/mrpt/maps/COctoMap.cpp +++ b/python/src/mrpt/maps/COctoMap.cpp @@ -991,9 +991,9 @@ struct PyCallBack_mrpt_maps_CSimplePointsMap : public mrpt::maps::CSimplePointsM } return CPointsMap::asString(); } - bool nn_supports_indices() const override { + bool nn_has_indices_or_ids() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_has_indices_or_ids"); if (overload) { auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { @@ -1002,7 +1002,7 @@ struct PyCallBack_mrpt_maps_CSimplePointsMap : public mrpt::maps::CSimplePointsM } else return pybind11::detail::cast_safe(std::move(o)); } - return CPointsMap::nn_supports_indices(); + return CPointsMap::nn_has_indices_or_ids(); } size_t nn_index_count() const override { pybind11::gil_scoped_acquire gil; @@ -1017,6 +1017,32 @@ struct PyCallBack_mrpt_maps_CSimplePointsMap : public mrpt::maps::CSimplePointsM } return CPointsMap::nn_index_count(); } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2, a3); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2, a3); + } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "PLY_import_set_face_count"); diff --git a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp index fa04eb7363..d4a317511a 100644 --- a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp +++ b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp @@ -555,9 +555,9 @@ struct PyCallBack_mrpt_maps_CPointsMapXYZI : public mrpt::maps::CPointsMapXYZI { } return CPointsMap::asString(); } - bool nn_supports_indices() const override { + bool nn_has_indices_or_ids() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_has_indices_or_ids"); if (overload) { auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { @@ -566,7 +566,7 @@ struct PyCallBack_mrpt_maps_CPointsMapXYZI : public mrpt::maps::CPointsMapXYZI { } else return pybind11::detail::cast_safe(std::move(o)); } - return CPointsMap::nn_supports_indices(); + return CPointsMap::nn_has_indices_or_ids(); } size_t nn_index_count() const override { pybind11::gil_scoped_acquire gil; @@ -581,6 +581,32 @@ struct PyCallBack_mrpt_maps_CPointsMapXYZI : public mrpt::maps::CPointsMapXYZI { } return CPointsMap::nn_index_count(); } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2, a3); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2, a3); + } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "PLY_import_set_face_count"); diff --git a/python/src/mrpt/maps/CPointsMap.cpp b/python/src/mrpt/maps/CPointsMap.cpp index 11fb2c5978..1b940df9a7 100644 --- a/python/src/mrpt/maps/CPointsMap.cpp +++ b/python/src/mrpt/maps/CPointsMap.cpp @@ -253,8 +253,10 @@ void bind_mrpt_maps_CPointsMap(std::function< pybind11::module &(std::string con cl.def("kdtree_distance", (float (mrpt::maps::CPointsMap::*)(const float *, size_t, size_t) const) &mrpt::maps::CPointsMap::kdtree_distance, "Returns the distance between the vector \"p1[0:size-1]\" and the data\n point with index \"idx_p2\" stored in the class:\n\nC++: mrpt::maps::CPointsMap::kdtree_distance(const float *, size_t, size_t) const --> float", pybind11::arg("p1"), pybind11::arg("idx_p2"), pybind11::arg("size")); cl.def("mark_as_modified", (void (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::mark_as_modified, "Users normally don't need to call this. Called by this class or children\n classes, set m_largestDistanceFromOriginIsUpdated=false, invalidates the\n kd-tree cache, and such. \n\nC++: mrpt::maps::CPointsMap::mark_as_modified() const --> void"); cl.def("asString", (std::string (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::CPointsMap::asString() const --> std::string"); - cl.def("nn_supports_indices", (bool (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::nn_supports_indices, " @{ \n\nC++: mrpt::maps::CPointsMap::nn_supports_indices() const --> bool"); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::nn_has_indices_or_ids, " @{ \n\nC++: mrpt::maps::CPointsMap::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::nn_index_count, "C++: mrpt::maps::CPointsMap::nn_index_count() const --> size_t"); + cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); { // mrpt::maps::CPointsMap::TInsertionOptions file:mrpt/maps/CPointsMap.h line:228 auto & enclosing_class = cl; diff --git a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp index 9504ee117d..918619c56d 100644 --- a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp +++ b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp @@ -824,9 +824,9 @@ struct PyCallBack_mrpt_maps_CWeightedPointsMap : public mrpt::maps::CWeightedPoi } return CPointsMap::asString(); } - bool nn_supports_indices() const override { + bool nn_has_indices_or_ids() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_has_indices_or_ids"); if (overload) { auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { @@ -835,7 +835,7 @@ struct PyCallBack_mrpt_maps_CWeightedPointsMap : public mrpt::maps::CWeightedPoi } else return pybind11::detail::cast_safe(std::move(o)); } - return CPointsMap::nn_supports_indices(); + return CPointsMap::nn_has_indices_or_ids(); } size_t nn_index_count() const override { pybind11::gil_scoped_acquire gil; @@ -850,6 +850,32 @@ struct PyCallBack_mrpt_maps_CWeightedPointsMap : public mrpt::maps::CWeightedPoi } return CPointsMap::nn_index_count(); } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2, a3); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CPointsMap::nn_single_search(a0, a1, a2, a3); + } void PLY_import_set_face_count(size_t a0) override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "PLY_import_set_face_count"); diff --git a/python/src/mrpt/maps/CVoxelMap.cpp b/python/src/mrpt/maps/CVoxelMap.cpp index 7a092cd008..60f6c61eaf 100644 --- a/python/src/mrpt/maps/CVoxelMap.cpp +++ b/python/src/mrpt/maps/CVoxelMap.cpp @@ -212,9 +212,9 @@ struct PyCallBack_mrpt_maps_CVoxelMap : public mrpt::maps::CVoxelMap { } return CVoxelMapOccupancyBase::boundingBox(); } - bool nn_supports_indices() const override { + bool nn_has_indices_or_ids() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_has_indices_or_ids"); if (overload) { auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { @@ -223,7 +223,7 @@ struct PyCallBack_mrpt_maps_CVoxelMap : public mrpt::maps::CVoxelMap { } else return pybind11::detail::cast_safe(std::move(o)); } - return CVoxelMapOccupancyBase::nn_supports_indices(); + return CVoxelMapOccupancyBase::nn_has_indices_or_ids(); } size_t nn_index_count() const override { pybind11::gil_scoped_acquire gil; @@ -238,6 +238,32 @@ struct PyCallBack_mrpt_maps_CVoxelMap : public mrpt::maps::CVoxelMap { } return CVoxelMapOccupancyBase::nn_index_count(); } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2, a3); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2, a3); + } void internal_clear() override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "internal_clear"); @@ -536,9 +562,9 @@ struct PyCallBack_mrpt_maps_CVoxelMapRGB : public mrpt::maps::CVoxelMapRGB { } return CVoxelMapOccupancyBase::boundingBox(); } - bool nn_supports_indices() const override { + bool nn_has_indices_or_ids() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_supports_indices"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_has_indices_or_ids"); if (overload) { auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { @@ -547,7 +573,7 @@ struct PyCallBack_mrpt_maps_CVoxelMapRGB : public mrpt::maps::CVoxelMapRGB { } else return pybind11::detail::cast_safe(std::move(o)); } - return CVoxelMapOccupancyBase::nn_supports_indices(); + return CVoxelMapOccupancyBase::nn_has_indices_or_ids(); } size_t nn_index_count() const override { pybind11::gil_scoped_acquire gil; @@ -562,6 +588,32 @@ struct PyCallBack_mrpt_maps_CVoxelMapRGB : public mrpt::maps::CVoxelMapRGB { } return CVoxelMapOccupancyBase::nn_index_count(); } + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2, a3); + } + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2, a3); + } void internal_clear() override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "internal_clear"); diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp index e0ce79c4aa..56901034ab 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp @@ -67,8 +67,10 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s cl.def_static("l2p", (float (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p(const signed char) --> float", pybind11::arg("l")); cl.def_static("l2p_255", (uint8_t (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p_255, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p_255(const signed char) --> uint8_t", pybind11::arg("l")); cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); - cl.def("nn_supports_indices", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_supports_indices, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_supports_indices() const --> bool"); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_index_count, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_index_count() const --> size_t"); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); @@ -99,8 +101,10 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s cl.def("squareDistanceToClosestCorrespondence", (float (mrpt::maps::CMetricMap::*)(float, float) const) &mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence, "Returns the square distance from the 2D point (x0,y0) to the closest\n correspondence in the map. \n\nC++: mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence(float, float) const --> float", pybind11::arg("x0"), pybind11::arg("y0")); cl.def("getAsSimplePointsMap", (class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMetricMap::getAsSimplePointsMap() --> class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); - cl.def("nn_supports_indices", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_supports_indices, "Returns true if the rest of `nn_*` methods will populate the indices\n std::optional<> return variables, false otherwise. \n\nC++: mrpt::maps::NearestNeighborsCapable::nn_supports_indices() const --> bool"); - cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_supports_indices() returns `true`, this must return the number of\n \"points\" (or whatever entity) the indices correspond to. Otherwise, the\n return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids, "Returns true if the rest of `nn_*` methods will populate the output\n indices values with 0-based contiguous **indices**.\n Returns false if indices are actually sparse **ID numbers** without any\n expectation of they be contiguous or start near zero.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_has_indices_or_ids() returns `true`, this must return the number\n of \"points\" (or whatever entity) the indices correspond to. Otherwise,\n the return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp index 79e816406a..48fc11853e 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp @@ -67,8 +67,10 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s cl.def_static("l2p", (float (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p(const signed char) --> float", pybind11::arg("l")); cl.def_static("l2p_255", (uint8_t (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p_255, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p_255(const signed char) --> uint8_t", pybind11::arg("l")); cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); - cl.def("nn_supports_indices", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_supports_indices, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_supports_indices() const --> bool"); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_index_count, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_index_count() const --> size_t"); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); @@ -99,8 +101,10 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s cl.def("squareDistanceToClosestCorrespondence", (float (mrpt::maps::CMetricMap::*)(float, float) const) &mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence, "Returns the square distance from the 2D point (x0,y0) to the closest\n correspondence in the map. \n\nC++: mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence(float, float) const --> float", pybind11::arg("x0"), pybind11::arg("y0")); cl.def("getAsSimplePointsMap", (class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMetricMap::getAsSimplePointsMap() --> class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); - cl.def("nn_supports_indices", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_supports_indices, "Returns true if the rest of `nn_*` methods will populate the indices\n std::optional<> return variables, false otherwise. \n\nC++: mrpt::maps::NearestNeighborsCapable::nn_supports_indices() const --> bool"); - cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_supports_indices() returns `true`, this must return the number of\n \"points\" (or whatever entity) the indices correspond to. Otherwise, the\n return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids, "Returns true if the rest of `nn_*` methods will populate the output\n indices values with 0-based contiguous **indices**.\n Returns false if indices are actually sparse **ID numbers** without any\n expectation of they be contiguous or start near zero.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_has_indices_or_ids() returns `true`, this must return the number\n of \"points\" (or whatever entity) the indices correspond to. Otherwise,\n the return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/maps/NearestNeighborsCapable.cpp b/python/src/mrpt/maps/NearestNeighborsCapable.cpp index 3edb04deb2..a22f929c1a 100644 --- a/python/src/mrpt/maps/NearestNeighborsCapable.cpp +++ b/python/src/mrpt/maps/NearestNeighborsCapable.cpp @@ -1,5 +1,13 @@ +#include +#include #include +#include +#include +#include +#include #include // __str__ +#include +#include #include #include @@ -16,10 +24,12 @@ void bind_mrpt_maps_NearestNeighborsCapable(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::NearestNeighborsCapable file:mrpt/maps/NearestNeighborsCapable.h line:28 + { // mrpt::maps::NearestNeighborsCapable file:mrpt/maps/NearestNeighborsCapable.h line:26 pybind11::class_> cl(M("mrpt::maps"), "NearestNeighborsCapable", "Virtual interface for maps having the capability of searching the closest\n neighbor(s) of a given query 2D or 3D point.\n\n Note this is more generic than mrpt::math::KDTreeCapable since it does not\n assume the use of KD-trees, and it is also non templatized, so users can use\n dynamic casting to interact with maps in a generic way.\n\n \n New in MRPT 2.11.3\n \n\n\n "); - cl.def("nn_supports_indices", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_supports_indices, "Returns true if the rest of `nn_*` methods will populate the indices\n std::optional<> return variables, false otherwise. \n\nC++: mrpt::maps::NearestNeighborsCapable::nn_supports_indices() const --> bool"); - cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_supports_indices() returns `true`, this must return the number of\n \"points\" (or whatever entity) the indices correspond to. Otherwise, the\n return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids, "Returns true if the rest of `nn_*` methods will populate the output\n indices values with 0-based contiguous **indices**.\n Returns false if indices are actually sparse **ID numbers** without any\n expectation of they be contiguous or start near zero.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_has_indices_or_ids() returns `true`, this must return the number\n of \"points\" (or whatever entity) the indices correspond to. Otherwise,\n the return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/slam/COccupancyGridMapFeatureExtractor.cpp b/python/src/mrpt/slam/COccupancyGridMapFeatureExtractor.cpp index c9b8abcc00..bb9779cb57 100644 --- a/python/src/mrpt/slam/COccupancyGridMapFeatureExtractor.cpp +++ b/python/src/mrpt/slam/COccupancyGridMapFeatureExtractor.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi index f90d7db557..ae626cfbc6 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi @@ -1753,11 +1753,15 @@ class COccupancyGridMap2D(CMetricMap, CLogOddsGridMap2D_signed_char_t, NearestNe @overload def loadFromBitmapFile(conststd, float, conststructmrpt) -> bool: ... def loadFromROSMapServerYAML(self, yamlFilePath: str) -> bool: ... + def nn_has_indices_or_ids(self) -> bool: ... @overload def nn_index_count(self) -> int: ... @overload def nn_index_count() -> size_t: ... - def nn_supports_indices(self) -> bool: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrID: int) -> bool: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrID: int) -> bool: ... def p2l(self, *args, **kwargs) -> Any: ... @overload def resizeGrid(self, new_x_min: float, new_x_max: float, new_y_min: float, new_y_max: float) -> None: ... @@ -1981,11 +1985,15 @@ class COccupancyGridMap3D(CMetricMap, CLogOddsGridMap3D_signed_char_t, NearestNe def isEmpty(self, *args, **kwargs) -> Any: ... def l2p(self, *args, **kwargs) -> Any: ... def l2p_255(self, *args, **kwargs) -> Any: ... + def nn_has_indices_or_ids(self) -> bool: ... @overload def nn_index_count(self) -> int: ... @overload def nn_index_count() -> size_t: ... - def nn_supports_indices(self) -> bool: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrID: int) -> bool: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrID: int) -> bool: ... def p2l(self, *args, **kwargs) -> Any: ... @overload def resizeGrid(self, corner_min, corner_max) -> None: ... @@ -2927,11 +2935,15 @@ class CPointsMap(CMetricMap, mrpt.pymrpt.mrpt.opengl.PLY_Importer, mrpt.pymrpt.m def mark_as_modified(self) -> None: ... @overload def mark_as_modified() -> void: ... + def nn_has_indices_or_ids(self) -> bool: ... @overload def nn_index_count(self) -> int: ... @overload def nn_index_count() -> size_t: ... - def nn_supports_indices(self) -> bool: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrID: int) -> bool: ... + @overload + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrID: int) -> bool: ... @overload def reserve(self, newLength: int) -> None: ... @overload @@ -3863,6 +3875,14 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t(CVoxelMapBa @overload def loadFromSimpleMap(constclassmrpt) -> void: ... @overload + def nn_has_indices_or_ids(self) -> bool: ... + @overload + def nn_has_indices_or_ids() -> bool: ... + @overload + def nn_has_indices_or_ids(self) -> bool: ... + @overload + def nn_has_indices_or_ids() -> bool: ... + @overload def nn_index_count(self) -> int: ... @overload def nn_index_count() -> size_t: ... @@ -3871,13 +3891,13 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t(CVoxelMapBa @overload def nn_index_count() -> size_t: ... @overload - def nn_supports_indices(self) -> bool: ... + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrID: int) -> bool: ... @overload - def nn_supports_indices() -> bool: ... + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrID: int) -> bool: ... @overload - def nn_supports_indices(self) -> bool: ... + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrIDOrID: int) -> bool: ... @overload - def nn_supports_indices() -> bool: ... + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrIDOrID: int) -> bool: ... def p2l(self, *args, **kwargs) -> Any: ... @overload def saveMetricMapRepresentationToFile(self, filNamePrefix: str) -> None: ... @@ -3977,6 +3997,14 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t(CVoxelMa @overload def loadFromSimpleMap(constclassmrpt) -> void: ... @overload + def nn_has_indices_or_ids(self) -> bool: ... + @overload + def nn_has_indices_or_ids() -> bool: ... + @overload + def nn_has_indices_or_ids(self) -> bool: ... + @overload + def nn_has_indices_or_ids() -> bool: ... + @overload def nn_index_count(self) -> int: ... @overload def nn_index_count() -> size_t: ... @@ -3985,13 +4013,13 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t(CVoxelMa @overload def nn_index_count() -> size_t: ... @overload - def nn_supports_indices(self) -> bool: ... + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrID: int) -> bool: ... @overload - def nn_supports_indices() -> bool: ... + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrID: int) -> bool: ... @overload - def nn_supports_indices(self) -> bool: ... + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrIDOrID: int) -> bool: ... @overload - def nn_supports_indices() -> bool: ... + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrIDOrID: int) -> bool: ... def p2l(self, *args, **kwargs) -> Any: ... @overload def saveMetricMapRepresentationToFile(self, filNamePrefix: str) -> None: ... @@ -4150,13 +4178,17 @@ class NearestNeighborsCapable: def __init__(self, *args, **kwargs) -> None: ... def assign(self) -> NearestNeighborsCapable: ... @overload + def nn_has_indices_or_ids(self) -> bool: ... + @overload + def nn_has_indices_or_ids() -> bool: ... + @overload def nn_index_count(self) -> int: ... @overload def nn_index_count() -> size_t: ... @overload - def nn_supports_indices(self) -> bool: ... + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrIDOrID: int) -> bool: ... @overload - def nn_supports_indices() -> bool: ... + def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrIDOrID: int) -> bool: ... class OccGridCellTraits: def __init__(self) -> None: ... From f8988a50effeb26f5ed2054e405af5672ac75575 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 6 Nov 2023 00:59:10 +0100 Subject: [PATCH 19/36] FIX: missing global map indices --- libs/maps/src/maps/CPointsMap.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/libs/maps/src/maps/CPointsMap.cpp b/libs/maps/src/maps/CPointsMap.cpp index 048de1e6da..e650ec6a2e 100644 --- a/libs/maps/src/maps/CPointsMap.cpp +++ b/libs/maps/src/maps/CPointsMap.cpp @@ -2106,7 +2106,7 @@ bool CPointsMap::nn_single_search( { try { - kdTreeClosestPoint3D( + resultIndexOrID = kdTreeClosestPoint3D( query.x, query.y, query.z, result.x, result.y, result.z, out_dist_sqr); return true; @@ -2123,7 +2123,7 @@ bool CPointsMap::nn_single_search( { try { - kdTreeClosestPoint2D( + resultIndexOrID = kdTreeClosestPoint2D( query.x, query.y, result.x, result.y, out_dist_sqr); return true; } @@ -2142,8 +2142,12 @@ void CPointsMap::nn_multiple_search( std::vector idxs; kdTreeNClosestPoint3DIdx(query.x, query.y, query.z, N, idxs, out_dists_sqr); results.resize(idxs.size()); + resultIndicesOrIDs.resize(idxs.size()); for (size_t i = 0; i < idxs.size(); i++) + { getPointFast(idxs[i], results[i].x, results[i].y, results[i].z); + resultIndicesOrIDs[i] = idxs[i]; + } } void CPointsMap::nn_multiple_search( const mrpt::math::TPoint2Df& query, const size_t N, @@ -2154,9 +2158,13 @@ void CPointsMap::nn_multiple_search( std::vector idxs; kdTreeNClosestPoint2DIdx(query.x, query.y, N, idxs, out_dists_sqr); results.resize(idxs.size()); + resultIndicesOrIDs.resize(idxs.size()); float dummyZ = 0; for (size_t i = 0; i < idxs.size(); i++) + { getPointFast(idxs[i], results[i].x, results[i].y, dummyZ); + resultIndicesOrIDs[i] = idxs[i]; + } } void CPointsMap::nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, @@ -2170,11 +2178,13 @@ void CPointsMap::nn_radius_search( const size_t nResults = indices_dist.size(); results.resize(nResults); out_dists_sqr.resize(nResults); + resultIndicesOrIDs.resize(nResults); for (size_t i = 0; i < nResults; i++) { getPointFast( indices_dist[i].first, results[i].x, results[i].y, results[i].z); out_dists_sqr[i] = indices_dist[i].second; + resultIndicesOrIDs[i] = indices_dist[i].first; } } void CPointsMap::nn_radius_search( @@ -2188,10 +2198,12 @@ void CPointsMap::nn_radius_search( const size_t nResults = indices_dist.size(); results.resize(nResults); out_dists_sqr.resize(nResults); + resultIndicesOrIDs.resize(nResults); float dummyZ = 0; for (size_t i = 0; i < nResults; i++) { getPointFast(indices_dist[i].first, results[i].x, results[i].y, dummyZ); out_dists_sqr[i] = indices_dist[i].second; + resultIndicesOrIDs[i] = indices_dist[i].first; } } From bd79cadff1976eda5a263dbdb1f8c8e50b6e2d2d Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 6 Nov 2023 09:40:46 +0100 Subject: [PATCH 20/36] Prefer anonymous namespace to static --- libs/config/src/CConfigFile.cpp | 5 ++- libs/containers/src/yaml.cpp | 37 ++++++++++++++++------ libs/core/src/Clock.cpp | 10 ++++-- libs/core/src/Clock_unittest.cpp | 5 ++- libs/core/src/WorkerThreadsPool.cpp | 5 ++- libs/core/src/exception_unittest.cpp | 5 ++- libs/core/src/exceptions.cpp | 5 ++- libs/img/src/CImage.cpp | 16 +++++++--- libs/img/src/CImage_loadXPM.cpp | 7 ++-- libs/img/src/CImage_unittest.cpp | 12 +++++-- libs/maps/src/obs/customizable_obs_viz.cpp | 5 ++- libs/math/src/geometry.cpp | 7 ++-- libs/obs/src/CObservation3DRangeScan.cpp | 15 ++++++--- libs/obs/src/CObservationIMU_unittest.cpp | 5 ++- libs/obs/src/CObservationVelodyneScan.cpp | 10 +++--- libs/obs/src/CSerializable_unittest.cpp | 13 +++++--- libs/obs/src/TMetricMapTypesRegistry.cpp | 5 ++- libs/obs/src/VelodyneCalibration.cpp | 12 ++++--- libs/opengl/src/CAssimpModel.cpp | 29 +++++++++-------- libs/opengl/src/CFBORender_unittest.cpp | 8 +++-- libs/opengl/src/CSkyBox.cpp | 2 +- libs/opengl/src/FrameBuffer.cpp | 5 ++- libs/opengl/src/RenderQueue.cpp | 5 ++- libs/opengl/src/TRenderMatrices.cpp | 5 ++- libs/opengl/src/Texture.cpp | 5 ++- libs/opengl/src/Viewport.cpp | 13 +++++--- libs/poses/src/CPose3DPDFGaussian.cpp | 5 ++- libs/rtti/src/CListOfClasses.cpp | 10 ++++-- libs/system/src/CTimeLogger_unittest.cpp | 5 ++- libs/system/src/datetime.cpp | 10 ++++-- libs/system/src/string_utils.cpp | 5 ++- libs/system/src/thread_name.cpp | 13 +++++--- libs/tfest/src/se2_l2.cpp | 5 ++- libs/tfest/src/se3_l2.cpp | 5 ++- 34 files changed, 219 insertions(+), 90 deletions(-) diff --git a/libs/config/src/CConfigFile.cpp b/libs/config/src/CConfigFile.cpp index 769756e598..1ae499fa1c 100644 --- a/libs/config/src/CConfigFile.cpp +++ b/libs/config/src/CConfigFile.cpp @@ -42,7 +42,9 @@ struct CConfigFile::Impl }; // copied from mrpt-io to avoid lib dependency: -static std::string local_file_get_contents(const std::string& fileName) +namespace +{ +std::string local_file_get_contents(const std::string& fileName) { // Credits: https://stackoverflow.com/a/2602258/1631514 // Note: Add "binary" to make sure the "tellg" file size matches the actual @@ -57,6 +59,7 @@ static std::string local_file_get_contents(const std::string& fileName) t.read(&buffer[0], size); return buffer; } +} // namespace /*--------------------------------------------------------------- Constructor diff --git a/libs/containers/src/yaml.cpp b/libs/containers/src/yaml.cpp index b99f318814..0408813c80 100644 --- a/libs/containers/src/yaml.cpp +++ b/libs/containers/src/yaml.cpp @@ -429,7 +429,9 @@ bool yaml::internalPrintNodeAsYAML( THROW_EXCEPTION("Should never reach here"); } -static std::string shortenComment(const std::optional& c) +namespace +{ +std::string shortenComment(const std::optional& c) { if (!c.has_value()) return {"[none]"}; @@ -439,6 +441,7 @@ static std::string shortenComment(const std::optional& c) else return {c.value()}; } +} // namespace void yaml::internalPrintDebugStructure( const node_t& p, std::ostream& o, int indent) @@ -499,7 +502,9 @@ void yaml::internalPrintDebugStructure( THROW_EXCEPTION("Should never reach here"); } -static void internalPrintRightComment(std::ostream& o, const std::string& c) +namespace +{ +void internalPrintRightComment(std::ostream& o, const std::string& c) { if (c.find('\n') == std::string::npos) { @@ -511,6 +516,7 @@ static void internalPrintRightComment(std::ostream& o, const std::string& c) s.erase(std::remove(s.begin(), s.end(), '\n'), s.end()); o << " # " << s << "\n"; } +} // namespace bool yaml::internalPrintAsYAML( const std::monostate&, std::ostream& o, const InternalPrintState& ps, @@ -637,7 +643,7 @@ bool yaml::internalPrintStringScalar( const std::string sInd(ps.indent + 2, ' '); // representation of empty *string* - const static std::string emptyStr = "''"; + const thread_local std::string emptyStr = "''"; const std::string& s = sIn.empty() ? emptyStr : sIn; const bool hasFinalNL = !s.empty() && s.back() == '\n'; @@ -770,7 +776,9 @@ yaml yaml::FromText(const std::string& yamlTextBlock) } // TODO: Allow users to add custom filters? -static yaml::scalar_t textToScalar(const std::string& s) +namespace +{ +yaml::scalar_t textToScalar(const std::string& s) { // tag:yaml.org,2002:null // https://yaml.org/spec/1.2/spec.html#id2803362 @@ -780,10 +788,13 @@ static yaml::scalar_t textToScalar(const std::string& s) return {s}; } +} // namespace #if MRPT_HAS_FYAML -static std::optional recursiveParse(struct fy_parser* p); - +namespace +{ +std::optional recursiveParse(struct fy_parser* p); +} static bool MRPT_YAML_PARSER_VERBOSE = mrpt::get_env("MRPT_YAML_PARSER_VERBOSE", false); @@ -796,7 +807,9 @@ static bool MRPT_YAML_PARSER_VERBOSE = } \ } while (0) -static std::optional extractComment( +namespace +{ +std::optional extractComment( struct fy_token* t, enum fy_comment_placement cp) { std::array str; @@ -813,7 +826,7 @@ static std::optional extractComment( return c; } -static void parseTokenCommentsAndMarks(struct fy_token* tk, yaml::node_t& n) +void parseTokenCommentsAndMarks(struct fy_token* tk, yaml::node_t& n) { if (!tk) return; @@ -839,7 +852,7 @@ static void parseTokenCommentsAndMarks(struct fy_token* tk, yaml::node_t& n) } } -static std::optional recursiveParse(struct fy_parser* p) +std::optional recursiveParse(struct fy_parser* p) { MRPT_START @@ -993,6 +1006,7 @@ static std::optional recursiveParse(struct fy_parser* p) #undef PARSER_DBG_OUT MRPT_END } +} // namespace #endif void yaml::loadFromText(const std::string& yamlTextBlock) @@ -1037,7 +1051,9 @@ yaml yaml::FromStream(std::istream& i) } // Replicated from mrpt::io to avoid dependency to that module: -static std::string local_file_get_contents(const std::string& fileName) +namespace +{ +std::string local_file_get_contents(const std::string& fileName) { // Credits: https://stackoverflow.com/a/2602258/1631514 // Note: Add "binary" to make sure the "tellg" file size matches the @@ -1055,6 +1071,7 @@ static std::string local_file_get_contents(const std::string& fileName) t.read(&buffer[0], size); return buffer; } +} // namespace void yaml::loadFromFile(const std::string& fileName) { diff --git a/libs/core/src/Clock.cpp b/libs/core/src/Clock.cpp index c2d1db2b84..110f519100 100644 --- a/libs/core/src/Clock.cpp +++ b/libs/core/src/Clock.cpp @@ -88,14 +88,19 @@ inline void from_nanoseconds(const uint64_t ns, struct timespec& ts) // Convert to TTimeStamp 100-nanoseconds representation: #if !defined(_WIN32) -static uint64_t to100ns(const timespec& tim) +namespace +{ +uint64_t to100ns(const timespec& tim) { return uint64_t(tim.tv_sec) * UINT64_C(10000000) + UINT64_C(116444736) * UINT64_C(1000000000) + tim.tv_nsec / 100; } +} // namespace #endif -static uint64_t getCurrentTime() noexcept +namespace +{ +uint64_t getCurrentTime() noexcept { const auto& clk = mrpt::internal::ClockState::Instance(); switch (clk.selectedClock()) @@ -163,6 +168,7 @@ static uint64_t getCurrentTime() noexcept return 0; // should never reach here } +} // namespace mrpt::Clock::time_point mrpt::Clock::now() noexcept { diff --git a/libs/core/src/Clock_unittest.cpp b/libs/core/src/Clock_unittest.cpp index f95425d39d..82b7e6e3a7 100644 --- a/libs/core/src/Clock_unittest.cpp +++ b/libs/core/src/Clock_unittest.cpp @@ -14,7 +14,9 @@ #include #include -static void test_delay() +namespace +{ +void test_delay() { const double t0 = mrpt::Clock::nowDouble(); std::this_thread::sleep_for(std::chrono::milliseconds(10)); @@ -23,6 +25,7 @@ static void test_delay() EXPECT_GT(t1 - t0, 0.008); // ideally, near 0.010 EXPECT_LT(t1 - t0, 5.0); // just detect it's not a crazy number } +} // namespace TEST(clock, delay_Realtime) { diff --git a/libs/core/src/WorkerThreadsPool.cpp b/libs/core/src/WorkerThreadsPool.cpp index c9dc3a5dcb..9ed52d13f3 100644 --- a/libs/core/src/WorkerThreadsPool.cpp +++ b/libs/core/src/WorkerThreadsPool.cpp @@ -85,7 +85,9 @@ void WorkerThreadsPool::resize(std::size_t num_threads) } // code partially replicated from mrpt::system for convenience (avoid dep) -static void mySetThreadName(const std::string& name, std::thread& theThread) +namespace +{ +void mySetThreadName(const std::string& name, std::thread& theThread) { #if defined(MRPT_OS_WINDOWS) && !defined(__MINGW32_MAJOR_VERSION) wchar_t wName[50]; @@ -96,6 +98,7 @@ static void mySetThreadName(const std::string& name, std::thread& theThread) pthread_setname_np(handle, name.c_str()); #endif } +} // namespace void WorkerThreadsPool::name(const std::string& name) { diff --git a/libs/core/src/exception_unittest.cpp b/libs/core/src/exception_unittest.cpp index be20e9f8f7..6c452da6d2 100644 --- a/libs/core/src/exception_unittest.cpp +++ b/libs/core/src/exception_unittest.cpp @@ -80,7 +80,9 @@ TEST(exception, assertException) mrpt::ExceptionWithCallBackBase); } -static std::string testFoo() +namespace +{ +std::string testFoo() { try { @@ -94,6 +96,7 @@ static std::string testFoo() return err; } } +} // namespace TEST(exception, infiniteRecurseBug) { diff --git a/libs/core/src/exceptions.cpp b/libs/core/src/exceptions.cpp index 4c6e67e8dd..8354019c99 100644 --- a/libs/core/src/exceptions.cpp +++ b/libs/core/src/exceptions.cpp @@ -29,7 +29,9 @@ std::string exception_line_msg( return s; } -static size_t findClosingBracket( +namespace +{ +size_t findClosingBracket( const char chClosing, const char chOpening, const std::string& str) { const size_t N = str.size(); @@ -46,6 +48,7 @@ static size_t findClosingBracket( } return std::string::npos; } +} // namespace /** Recursive implementation for mrpt::exception_to_str() */ void impl_excep_to_str( diff --git a/libs/img/src/CImage.cpp b/libs/img/src/CImage.cpp index d6d94cae83..ba6132e8c1 100644 --- a/libs/img/src/CImage.cpp +++ b/libs/img/src/CImage.cpp @@ -96,7 +96,9 @@ mrpt::img::CTimeLogger alloc_tims; #endif #if MRPT_HAS_OPENCV -static int interpolationMethod2Cv(TInterpolationMethod i) +namespace +{ +int interpolationMethod2Cv(TInterpolationMethod i) { // clang-format off switch (i) @@ -109,6 +111,7 @@ static int interpolationMethod2Cv(TInterpolationMethod i) // clang-format on return -1; } +} // namespace template constexpr RET pixelDepth2CvDepth(PixelDepth d) @@ -145,7 +148,9 @@ RET pixelDepth2IPLCvDepth(PixelDepth d) return std::numeric_limits::max(); } -static PixelDepth cvDepth2PixelDepth(int64_t d) +namespace +{ +PixelDepth cvDepth2PixelDepth(int64_t d) { // clang-format off switch (d) @@ -161,7 +166,7 @@ static PixelDepth cvDepth2PixelDepth(int64_t d) // clang-format on return PixelDepth::D8U; } - +} // namespace #endif // MRPT_HAS_OPENCV // Default ctor @@ -982,7 +987,9 @@ CImage CImage::grayscale() const // Auxiliary function for both ::grayscale() and ::grayscaleInPlace() #if MRPT_HAS_OPENCV -static bool my_img_to_grayscale(const cv::Mat& src, cv::Mat& dest) +namespace +{ +bool my_img_to_grayscale(const cv::Mat& src, cv::Mat& dest) { if (dest.size() != src.size() || dest.type() != src.type()) dest = cv::Mat(src.rows, src.cols, CV_8UC1); @@ -1003,6 +1010,7 @@ static bool my_img_to_grayscale(const cv::Mat& src, cv::Mat& dest) cv::cvtColor(src, dest, CV_BGR2GRAY); return false; } +} // namespace #endif bool CImage::grayscale(CImage& ret) const diff --git a/libs/img/src/CImage_loadXPM.cpp b/libs/img/src/CImage_loadXPM.cpp index 75a5799c32..d62a4f6b3d 100644 --- a/libs/img/src/CImage_loadXPM.cpp +++ b/libs/img/src/CImage_loadXPM.cpp @@ -367,7 +367,9 @@ static const rgbRecord theRGBRecords[] = { {nullptr, myRGB(0, 0, 0)}}; static const int numTheRGBRecords = 235; -static unsigned char ParseHexadecimal(char digit1, char digit2) +namespace +{ +unsigned char ParseHexadecimal(char digit1, char digit2) { unsigned char i1, i2; @@ -384,7 +386,7 @@ static unsigned char ParseHexadecimal(char digit1, char digit2) return (unsigned char)(0x10 * i1 + i2); } -static bool GetRGBFromName( +bool GetRGBFromName( const char* inname, bool* isNone, unsigned char* r, unsigned char* g, unsigned char* b) { @@ -475,6 +477,7 @@ static bool GetRGBFromName( return found; } +} // namespace #endif /** Loads the image from an XPM array, as included from a ".xpm" file. diff --git a/libs/img/src/CImage_unittest.cpp b/libs/img/src/CImage_unittest.cpp index 2008a00264..f623117f2e 100644 --- a/libs/img/src/CImage_unittest.cpp +++ b/libs/img/src/CImage_unittest.cpp @@ -29,7 +29,9 @@ const auto tstImgFileColor = mrpt::UNITTEST_BASEDIR() + "/samples/img_basic_example/frame_color.jpg"s; // Generate random img: -static void fillImagePseudoRandom(uint32_t seed, mrpt::img::CImage& img) +namespace +{ +void fillImagePseudoRandom(uint32_t seed, mrpt::img::CImage& img) { mrpt::random::Randomize(seed); auto& rnd = mrpt::random::getRandomGenerator(); @@ -45,7 +47,7 @@ static void fillImagePseudoRandom(uint32_t seed, mrpt::img::CImage& img) } // Expect images to be identical: -static bool expect_identical( +bool expect_identical( const mrpt::img::CImage& a, const mrpt::img::CImage& b, const std::string& s = std::string()) { @@ -59,6 +61,7 @@ static bool expect_identical( } return true; } +} // namespace TEST(CImage, CtorDefault) { @@ -72,7 +75,9 @@ TEST(CImage, CtorDefault) #if MRPT_HAS_OPENCV -static void CtorSized_gray(unsigned int w, unsigned int h) +namespace +{ +void CtorSized_gray(unsigned int w, unsigned int h) { using namespace mrpt::img; CImage img(w, h, CH_GRAY); @@ -82,6 +87,7 @@ static void CtorSized_gray(unsigned int w, unsigned int h) EXPECT_EQ(img.getPixelDepth(), PixelDepth::D8U); EXPECT_FALSE(img.isColor()); } +} // namespace TEST(CImage, CtorSized) { diff --git a/libs/maps/src/obs/customizable_obs_viz.cpp b/libs/maps/src/obs/customizable_obs_viz.cpp index 7fbee5f32b..775a37afef 100644 --- a/libs/maps/src/obs/customizable_obs_viz.cpp +++ b/libs/maps/src/obs/customizable_obs_viz.cpp @@ -133,7 +133,9 @@ void mrpt::obs::recolorize3Dpc( } } -static void add_common_to_viz( +namespace +{ +void add_common_to_viz( const CObservation& obs, const VisualizationParameters& p, mrpt::opengl::CSetOfObjects& out) { @@ -168,6 +170,7 @@ static void add_common_to_viz( out.insert(glCorner); } } +} // namespace void mrpt::obs::obs3Dscan_to_viz( const CObservation3DRangeScan::Ptr& obs, const VisualizationParameters& p, diff --git a/libs/math/src/geometry.cpp b/libs/math/src/geometry.cpp index 08138a6c87..e02d17adc9 100644 --- a/libs/math/src/geometry.cpp +++ b/libs/math/src/geometry.cpp @@ -1968,7 +1968,9 @@ void math::assemblePolygons( if (!usedSegments[i]) remainder.push_back(tmp[i]); } -static std::vector getPolygons( +namespace +{ +std::vector getPolygons( const std::vector& objs, const mrpt::optional_ref>& others = std::nullopt) @@ -1982,7 +1984,7 @@ static std::vector getPolygons( } return r; } -static std::vector getSegments( +std::vector getSegments( const std::vector& objs, const mrpt::optional_ref>& others = std::nullopt) @@ -1997,6 +1999,7 @@ static std::vector getSegments( } return r; } +} // namespace void math::assemblePolygons( const std::vector& objs, std::vector& polys) diff --git a/libs/obs/src/CObservation3DRangeScan.cpp b/libs/obs/src/CObservation3DRangeScan.cpp index b34e63f62a..2ae811b62c 100644 --- a/libs/obs/src/CObservation3DRangeScan.cpp +++ b/libs/obs/src/CObservation3DRangeScan.cpp @@ -263,7 +263,9 @@ using TMyRangesMemPool = mrpt::system::CGenericMemoryPool< CObservation3DRangeScan_Ranges_MemPoolParams, CObservation3DRangeScan_Ranges_MemPoolData>; -static void mempool_donate_xyz_buffers(CObservation3DRangeScan& obs) +namespace +{ +void mempool_donate_xyz_buffers(CObservation3DRangeScan& obs) { if (obs.points3D_x.empty()) return; // Before dying, donate my memory to the pool for the joy of future @@ -291,6 +293,8 @@ static void mempool_donate_xyz_buffers(CObservation3DRangeScan& obs) pool->dump_to_pool(mem_params, mem_block); } +} // namespace + void mempool_donate_range_matrix(CObservation3DRangeScan& obs) { if (obs.rangeImage.cols() == 0 || obs.rangeImage.rows() == 0) return; @@ -912,7 +916,9 @@ struct TLevMarData } }; -static void cam2vec(const TCamera& camPar, CVectorDouble& x) +namespace +{ +void cam2vec(const TCamera& camPar, CVectorDouble& x) { if (x.size() < 4 + 4) x.resize(4 + 4); @@ -924,7 +930,7 @@ static void cam2vec(const TCamera& camPar, CVectorDouble& x) for (size_t i = 0; i < 4; i++) x[4 + i] = camPar.dist[i]; } -static void vec2cam(const CVectorDouble& x, TCamera& camPar) +void vec2cam(const CVectorDouble& x, TCamera& camPar) { camPar.intrinsicParams(0, 0) = x[0]; // fx camPar.intrinsicParams(1, 1) = x[1]; // fy @@ -934,7 +940,7 @@ static void vec2cam(const CVectorDouble& x, TCamera& camPar) for (size_t i = 0; i < 4; i++) camPar.dist[i] = x[4 + i]; } -static void cost_func( +void cost_func( const CVectorDouble& par, const TLevMarData& d, CVectorDouble& err) { const CObservation3DRangeScan& obs = d.obs; @@ -991,6 +997,7 @@ static void cost_func( } } } // end error_func +} // namespace } // namespace mrpt::obs::detail /** A Levenberg-Marquart-based optimizer to recover the calibration parameters diff --git a/libs/obs/src/CObservationIMU_unittest.cpp b/libs/obs/src/CObservationIMU_unittest.cpp index 22a9d73c1c..99eec7d2a9 100644 --- a/libs/obs/src/CObservationIMU_unittest.cpp +++ b/libs/obs/src/CObservationIMU_unittest.cpp @@ -15,7 +15,9 @@ #include #include -static void checkExpectedValues(const mrpt::obs::CObservationIMU& o) +namespace +{ +void checkExpectedValues(const mrpt::obs::CObservationIMU& o) { EXPECT_FALSE(o.has(mrpt::obs::IMU_X)); @@ -27,6 +29,7 @@ static void checkExpectedValues(const mrpt::obs::CObservationIMU& o) EXPECT_NEAR(o.get(mrpt::obs::IMU_WY), 0.0, 1e-4); EXPECT_NEAR(o.get(mrpt::obs::IMU_WZ), 0.00222222, 1e-4); } +} // namespace TEST(CObservationIMU, Deserialize_v3) { diff --git a/libs/obs/src/CObservationVelodyneScan.cpp b/libs/obs/src/CObservationVelodyneScan.cpp index 005e2f1f71..83a5717f67 100644 --- a/libs/obs/src/CObservationVelodyneScan.cpp +++ b/libs/obs/src/CObservationVelodyneScan.cpp @@ -122,19 +122,20 @@ void Velo::getDescriptionAsText(std::ostream& o) const } /** [us] */ -static double HDL32AdjustTimeStamp(int firingblock, int dsr) +namespace +{ +double HDL32AdjustTimeStamp(int firingblock, int dsr) { return (firingblock * HDR32_FIRING_TOFFSET) + (dsr * HDR32_DSR_TOFFSET); } /** [us] */ -static double VLP16AdjustTimeStamp( - int firingblock, int dsr, int firingwithinblock) +double VLP16AdjustTimeStamp(int firingblock, int dsr, int firingwithinblock) { return (firingblock * VLP16_BLOCK_TDURATION) + (dsr * VLP16_DSR_TOFFSET) + (firingwithinblock * VLP16_FIRING_TOFFSET); } -static void velodyne_scan_to_pointcloud( +void velodyne_scan_to_pointcloud( const Velo& scan, const Velo::TGeneratePointCloudParameters& params, Velo::PointCloudStorageWrapper& out_pc) { @@ -419,6 +420,7 @@ static void velodyne_scan_to_pointcloud( } // end for each block [0,11] } // end for each data packet } +} // namespace void Velo::generatePointCloud( PointCloudStorageWrapper& dest, const TGeneratePointCloudParameters& params) diff --git a/libs/obs/src/CSerializable_unittest.cpp b/libs/obs/src/CSerializable_unittest.cpp index aa4642b4ef..1a5cc4bbbd 100644 --- a/libs/obs/src/CSerializable_unittest.cpp +++ b/libs/obs/src/CSerializable_unittest.cpp @@ -129,25 +129,28 @@ TEST(Observations, WriteReadToOctectVectors) } } -static bool aux_get_sample_data(mrpt::obs::CObservation&) { return false; } -static bool aux_get_sample_data(mrpt::obs::CAction&) { return false; } +namespace +{ +bool aux_get_sample_data(mrpt::obs::CObservation&) { return false; } +bool aux_get_sample_data(mrpt::obs::CAction&) { return false; } -static bool aux_get_sample_data(mrpt::obs::CObservation2DRangeScan& o) +bool aux_get_sample_data(mrpt::obs::CObservation2DRangeScan& o) { mrpt::obs::stock_observations::example2DRangeScan(o); return true; } -static bool aux_get_sample_data(mrpt::obs::CObservationImage& o) +bool aux_get_sample_data(mrpt::obs::CObservationImage& o) { mrpt::obs::stock_observations::exampleImage(o.image); return true; } -static bool aux_get_sample_data(mrpt::obs::CObservationStereoImages& o) +bool aux_get_sample_data(mrpt::obs::CObservationStereoImages& o) { mrpt::obs::stock_observations::exampleImage(o.imageLeft, 0); mrpt::obs::stock_observations::exampleImage(o.imageRight, 1); return true; } +} // namespace // Try to invoke a copy ctor and = operator: template diff --git a/libs/obs/src/TMetricMapTypesRegistry.cpp b/libs/obs/src/TMetricMapTypesRegistry.cpp index 2026e48fec..bff5a65244 100644 --- a/libs/obs/src/TMetricMapTypesRegistry.cpp +++ b/libs/obs/src/TMetricMapTypesRegistry.cpp @@ -25,7 +25,9 @@ TMetricMapTypesRegistry& TMetricMapTypesRegistry::Instance() return reg; } -static std::string stripNamespace(const std::string& n) +namespace +{ +std::string stripNamespace(const std::string& n) { std::string ret = n; const auto pos = ret.rfind("::"); @@ -33,6 +35,7 @@ static std::string stripNamespace(const std::string& n) return ret; } +} // namespace size_t TMetricMapTypesRegistry::doRegister( const std::string& names, MapDefCtorFunctor func1, diff --git a/libs/obs/src/VelodyneCalibration.cpp b/libs/obs/src/VelodyneCalibration.cpp index ba7e779317..f2687156a0 100644 --- a/libs/obs/src/VelodyneCalibration.cpp +++ b/libs/obs/src/VelodyneCalibration.cpp @@ -36,7 +36,9 @@ using namespace mrpt::obs; VelodyneCalibration::PerLaserCalib::PerLaserCalib() = default; #if MRPT_HAS_TINYXML2 -static const tinyxml2::XMLElement* get_xml_children( +namespace +{ +const tinyxml2::XMLElement* get_xml_children( const tinyxml2::XMLNode* e, const char* name) { ASSERT_(e != nullptr); @@ -48,7 +50,7 @@ static const tinyxml2::XMLElement* get_xml_children( mrpt::format("Cannot find XML tag `%s`", name)); return ret; } -static const char* get_xml_children_as_str( +const char* get_xml_children_as_str( const tinyxml2::XMLNode* e, const char* name) { auto n = get_xml_children(e, name); @@ -58,15 +60,15 @@ static const char* get_xml_children_as_str( txt, mrpt::format("Cannot convert XML tag `%s` to string", name)); return txt; } -static double get_xml_children_as_double( - const tinyxml2::XMLNode* e, const char* name) +double get_xml_children_as_double(const tinyxml2::XMLNode* e, const char* name) { return ::atof(get_xml_children_as_str(e, name)); } -static int get_xml_children_as_int(const tinyxml2::XMLNode* e, const char* name) +int get_xml_children_as_int(const tinyxml2::XMLNode* e, const char* name) { return ::atoi(get_xml_children_as_str(e, name)); } +} // namespace #endif VelodyneCalibration::VelodyneCalibration() : laser_corrections(0) {} diff --git a/libs/opengl/src/CAssimpModel.cpp b/libs/opengl/src/CAssimpModel.cpp index f0df002ef6..b3cf571e13 100644 --- a/libs/opengl/src/CAssimpModel.cpp +++ b/libs/opengl/src/CAssimpModel.cpp @@ -154,14 +154,16 @@ struct CAssimpModel::Impl #if (MRPT_HAS_OPENGL_GLUT || MRPT_HAS_EGL) && MRPT_HAS_ASSIMP // Just return the diffuse color: -static mrpt::img::TColor apply_material( +namespace +{ +mrpt::img::TColor apply_material( const aiMaterial* mtl, const img::TColor& defaultColor, const bool ignoreMaterialColor); -static void get_bounding_box( - const aiScene* sc, aiVector3D* min, aiVector3D* max); -static void get_bounding_box_for_node( +void get_bounding_box(const aiScene* sc, aiVector3D* min, aiVector3D* max); +void get_bounding_box_for_node( const aiScene* sc, const aiNode* nd, aiVector3D* min, aiVector3D* max, aiMatrix4x4* trafo); +} // namespace #endif // MRPT_HAS_OPENGL_GLUT && MRPT_HAS_ASSIMP void CAssimpModel::render(const RenderContext& rc) const @@ -438,7 +440,9 @@ bool CAssimpModel::traceRay( #if (MRPT_HAS_OPENGL_GLUT || MRPT_HAS_EGL) && MRPT_HAS_ASSIMP -static void get_bounding_box_for_node( +namespace +{ +void get_bounding_box_for_node( const aiScene* scene, const aiNode* nd, aiVector3D* min, aiVector3D* max, aiMatrix4x4* trafo) { @@ -473,8 +477,7 @@ static void get_bounding_box_for_node( *trafo = prev; } -static void get_bounding_box( - const aiScene* scene, aiVector3D* min, aiVector3D* max) +void get_bounding_box(const aiScene* scene, aiVector3D* min, aiVector3D* max) { aiMatrix4x4 trafo; aiIdentityMatrix4(&trafo); @@ -484,12 +487,12 @@ static void get_bounding_box( get_bounding_box_for_node(scene, scene->mRootNode, min, max, &trafo); } -static mrpt::img::TColor color4_to_TColor(const aiColor4D& c) +mrpt::img::TColor color4_to_TColor(const aiColor4D& c) { return mrpt::img::TColorf(c.r, c.g, c.b, c.a).asTColor(); } -static mrpt::img::TColor apply_material( +mrpt::img::TColor apply_material( const aiMaterial* mtl, const mrpt::img::TColor& defaultColor, const bool ignoreMaterialColor) { @@ -506,7 +509,7 @@ static mrpt::img::TColor apply_material( } } -static mrpt::math::CMatrixDouble44 aiMatrix_to_mrpt(const aiMatrix4x4& m) +mrpt::math::CMatrixDouble44 aiMatrix_to_mrpt(const aiMatrix4x4& m) { mrpt::math::CMatrixDouble44 M; M(0, 0) = m.a1; @@ -531,10 +534,8 @@ static mrpt::math::CMatrixDouble44 aiMatrix_to_mrpt(const aiMatrix4x4& m) return M; } -static mrpt::math::TPoint3Df to_mrpt(const aiVector3D& v) -{ - return {v.x, v.y, v.z}; -} +mrpt::math::TPoint3Df to_mrpt(const aiVector3D& v) { return {v.x, v.y, v.z}; } +} // namespace void CAssimpModel::recursive_render( const aiScene* sc, const aiNode* nd, const mrpt::poses::CPose3D& transf, diff --git a/libs/opengl/src/CFBORender_unittest.cpp b/libs/opengl/src/CFBORender_unittest.cpp index a61d44075d..f30e6119e9 100644 --- a/libs/opengl/src/CFBORender_unittest.cpp +++ b/libs/opengl/src/CFBORender_unittest.cpp @@ -47,8 +47,9 @@ #undef RUN_OFFSCREEN_RENDER_TESTS #endif -static float imageDiff( - const mrpt::img::CImage& im1, const mrpt::img::CImage& im2) +namespace +{ +float imageDiff(const mrpt::img::CImage& im1, const mrpt::img::CImage& im2) { mrpt::math::CMatrixFloat r1, g1, b1; im1.getAsRGBMatrices(r1, g1, b1); @@ -61,7 +62,7 @@ static float imageDiff( (b1 - b2).asEigen().array().abs().sum(); } -static void test_opengl_CFBORender(const bool useCameraFromIntrinsics) +void test_opengl_CFBORender(const bool useCameraFromIntrinsics) { using namespace mrpt; // _deg using namespace std::string_literals; // s @@ -251,6 +252,7 @@ static void test_opengl_CFBORender(const bool useCameraFromIntrinsics) EXPECT_LT(depth_diff, 3000.0f); } } +} // namespace #if defined(RUN_OFFSCREEN_RENDER_TESTS) TEST(OpenGL, CFBORender_camera_intrinsics) diff --git a/libs/opengl/src/CSkyBox.cpp b/libs/opengl/src/CSkyBox.cpp index 135d064759..e45baf912a 100644 --- a/libs/opengl/src/CSkyBox.cpp +++ b/libs/opengl/src/CSkyBox.cpp @@ -27,7 +27,7 @@ IMPLEMENTS_SERIALIZABLE(CSkyBox, CRenderizable, mrpt::opengl) void CSkyBox::renderUpdateBuffers() const { // vertices: - static constexpr float skyboxVertices[] = { + constexpr thread_local float skyboxVertices[] = { // Positions -1.0f, 1.0f, -1.0f, // -1.0f, -1.0f, -1.0f, // diff --git a/libs/opengl/src/FrameBuffer.cpp b/libs/opengl/src/FrameBuffer.cpp index 186c34fdcc..4905e35fe9 100644 --- a/libs/opengl/src/FrameBuffer.cpp +++ b/libs/opengl/src/FrameBuffer.cpp @@ -17,7 +17,9 @@ using namespace mrpt::opengl; #if MRPT_HAS_OPENGL_GLUT || MRPT_HAS_EGL -static bool isExtensionSupported([[maybe_unused]] const std::string& extension) +namespace +{ +bool isExtensionSupported(const std::string& extension) { MRPT_START for (int index = 0;; index++) @@ -31,6 +33,7 @@ static bool isExtensionSupported([[maybe_unused]] const std::string& extension) MRPT_END return false; } +} // namespace #endif void FrameBuffer::RAII_Impl::create( diff --git a/libs/opengl/src/RenderQueue.cpp b/libs/opengl/src/RenderQueue.cpp index 5beb2ac864..fa270fa868 100644 --- a/libs/opengl/src/RenderQueue.cpp +++ b/libs/opengl/src/RenderQueue.cpp @@ -36,7 +36,9 @@ using namespace mrpt::opengl; // |(-1,-1) (+1,-1)| // +----------------------+ // -static std::tuple projectToScreenCoordsAndDepth( +namespace +{ +std::tuple projectToScreenCoordsAndDepth( const mrpt::math::TPoint3Df& localPt, const mrpt::opengl::TRenderMatrices& objState) { @@ -64,6 +66,7 @@ static std::tuple projectToScreenCoordsAndDepth( return {uv, depth}; } +} // namespace // See docs in .h std::tuple mrpt::opengl::depthAndVisibleInView( diff --git a/libs/opengl/src/TRenderMatrices.cpp b/libs/opengl/src/TRenderMatrices.cpp index a17a10d669..2b412bb46a 100644 --- a/libs/opengl/src/TRenderMatrices.cpp +++ b/libs/opengl/src/TRenderMatrices.cpp @@ -139,7 +139,9 @@ void TRenderMatrices::computeProjectionMatrix(float znear, float zfar) } } -static void azimuthElevationFromDirection( +namespace +{ +void azimuthElevationFromDirection( const mrpt::math::TVector3Df& v, float& elevation, float& azimuth) { // Compute the elevation angle @@ -150,6 +152,7 @@ static void azimuthElevationFromDirection( else azimuth = atan2(v.y, v.x); } +} // namespace void TRenderMatrices::computeLightProjectionMatrix( float zmin, float zmax, const TLightParameters& lp) diff --git a/libs/opengl/src/Texture.cpp b/libs/opengl/src/Texture.cpp index e101dc942c..576599c1b7 100644 --- a/libs/opengl/src/Texture.cpp +++ b/libs/opengl/src/Texture.cpp @@ -204,7 +204,9 @@ using TMyMemPool = mrpt::system::CGenericMemoryPool< // Auxiliary function for loadTextureInOpenGL(): reserve memory and return // 16byte aligned starting point within it: -static unsigned char* reserveDataBuffer(size_t len, std::vector& data) +namespace +{ +unsigned char* reserveDataBuffer(size_t len, std::vector& data) { #ifdef TEXTUREOBJ_USE_MEMPOOL TMyMemPool* pool = TMyMemPool::getInstance(); @@ -229,6 +231,7 @@ static unsigned char* reserveDataBuffer(size_t len, std::vector& data) return reinterpret_cast( std::align(16, 1 /*dummy size*/, ptr, space)); } +} // namespace void Texture::internalAssignImage_2D( const mrpt::img::CImage* in_rgb, const mrpt::img::CImage* in_alpha, diff --git a/libs/opengl/src/Viewport.cpp b/libs/opengl/src/Viewport.cpp index fcf171e0ea..058100b078 100644 --- a/libs/opengl/src/Viewport.cpp +++ b/libs/opengl/src/Viewport.cpp @@ -106,8 +106,9 @@ void Viewport::insert(const CRenderizable::Ptr& newObject) } // Maps [0,1] to [0,Len], wrap negative numbers, etc. -static int sizeFromRatio( - const int startCoord, const double dSize, const int iLength) +namespace +{ +int sizeFromRatio(const int startCoord, const double dSize, const int iLength) { if (dSize > 1) // >1 -> absolute pixels: return static_cast(dSize); @@ -122,7 +123,7 @@ static int sizeFromRatio( // Otherwise: a fraction return static_cast(iLength * dSize); } -static int startFromRatio(const double frac, const int dSize) +int startFromRatio(const double frac, const int dSize) { const bool doWrap = (frac < 0); const auto fracAbs = std::abs(frac); @@ -134,6 +135,7 @@ static int startFromRatio(const double frac, const int dSize) return ret; } +} // namespace // "Image mode" rendering: void Viewport::renderImageMode() const @@ -274,7 +276,9 @@ void Viewport::loadDefaultShaders() const // ----------------------------------------- static unsigned int quadVAO = 0; static unsigned int quadVBO; -static void debugRenderQuad() +namespace +{ + void debugRenderQuad() { if (quadVAO == 0) { @@ -308,6 +312,7 @@ static void debugRenderQuad() glDrawArrays(GL_TRIANGLE_STRIP, 0, 4); glBindVertexArray(0); } +} // namespace #endif /** Render a normal scene with 3D objects */ diff --git a/libs/poses/src/CPose3DPDFGaussian.cpp b/libs/poses/src/CPose3DPDFGaussian.cpp index b12e68074a..7d2e461dc6 100644 --- a/libs/poses/src/CPose3DPDFGaussian.cpp +++ b/libs/poses/src/CPose3DPDFGaussian.cpp @@ -102,7 +102,9 @@ void ffff( } #endif -static void aux_posequat2poseypr( +namespace +{ +void aux_posequat2poseypr( const CVectorFixedDouble<7>& x, [[maybe_unused]] const double& dummy, CVectorFixedDouble<6>& y) { @@ -113,6 +115,7 @@ static void aux_posequat2poseypr( q.normalize(); q.rpy(y[5], y[4], y[3]); } +} // namespace /*--------------------------------------------------------------- CPose3DPDFGaussian diff --git a/libs/rtti/src/CListOfClasses.cpp b/libs/rtti/src/CListOfClasses.cpp index e9af1d87e1..18275fd345 100644 --- a/libs/rtti/src/CListOfClasses.cpp +++ b/libs/rtti/src/CListOfClasses.cpp @@ -40,7 +40,9 @@ std::string CListOfClasses::asString() const } // trim from start -static inline std::string& ltrim(std::string& s) +namespace +{ +inline std::string& ltrim(std::string& s) { s.erase(s.begin(), std::find_if(s.begin(), s.end(), [](const char c) { return !std::isspace(c); @@ -49,7 +51,7 @@ static inline std::string& ltrim(std::string& s) } // trim from end -static inline std::string& rtrim(std::string& s) +inline std::string& rtrim(std::string& s) { s.erase( std::find_if( @@ -60,7 +62,9 @@ static inline std::string& rtrim(std::string& s) } // trim from both ends -static inline std::string& trim(std::string& s) { return ltrim(rtrim(s)); } +inline std::string& trim(std::string& s) { return ltrim(rtrim(s)); } +} // namespace + void CListOfClasses::fromString(const std::string& s) { MRPT_TRY_START diff --git a/libs/system/src/CTimeLogger_unittest.cpp b/libs/system/src/CTimeLogger_unittest.cpp index 7685a79e35..306d811f7a 100644 --- a/libs/system/src/CTimeLogger_unittest.cpp +++ b/libs/system/src/CTimeLogger_unittest.cpp @@ -14,13 +14,16 @@ #include #include -static void doTimLogEntry( +namespace +{ +void doTimLogEntry( mrpt::system::CTimeLogger& tl, const char* name, const int ms) { tl.enter(name); std::this_thread::sleep_for(std::chrono::milliseconds(ms)); tl.leave(name); } +} // namespace TEST(CTimeLogger, getLastTime) { diff --git a/libs/system/src/datetime.cpp b/libs/system/src/datetime.cpp index 8a5cfb7c1b..ec2760edb1 100644 --- a/libs/system/src/datetime.cpp +++ b/libs/system/src/datetime.cpp @@ -160,11 +160,14 @@ string mrpt::system::formatTimeInterval(const double t) return s; } -static unsigned int calcSecFractions(const uint64_t tmp) +namespace +{ +unsigned int calcSecFractions(const uint64_t tmp) { return static_cast( 1e6 * static_cast(tmp % 10000000) / 1e7); } +} // namespace /*--------------------------------------------------------------- Convert a timestamp into this textual form: YEAR/MONTH/DAY,HH:MM:SS.MMM @@ -306,7 +309,9 @@ string mrpt::system::dateToString(const mrpt::system::TTimeStamp tt) "%u/%02u/%02u", 1900 + ptm->tm_year, ptm->tm_mon + 1, ptm->tm_mday); } -static std::string implIntervalFormat(const double seconds) +namespace +{ +std::string implIntervalFormat(const double seconds) { using namespace std::string_literals; @@ -343,6 +348,7 @@ static std::string implIntervalFormat(const double seconds) else return format("%.2f ns", seconds * 1e9); } +} // namespace std::string mrpt::system::intervalFormat(const double seconds) { diff --git a/libs/system/src/string_utils.cpp b/libs/system/src/string_utils.cpp index f344ccdd7e..1d09731baf 100644 --- a/libs/system/src/string_utils.cpp +++ b/libs/system/src/string_utils.cpp @@ -320,8 +320,10 @@ bool mrpt::system::strStartsI(const std::string& s1, const std::string& s2) s2.size()); // if s1 is shorter it's not a problem } +namespace +{ template -static void impl_stringListAsString( +void impl_stringListAsString( const STRING_LIST& lst, std::string& outText, const std::string& newline) { const size_t lenNL = newline.size(); @@ -344,6 +346,7 @@ static void impl_stringListAsString( outText[curPos++] = sNL; } } +} // namespace void mrpt::system::stringListAsString( const std::vector& lst, std::string& outText, diff --git a/libs/system/src/thread_name.cpp b/libs/system/src/thread_name.cpp index 85ea9a4b94..882f838adb 100644 --- a/libs/system/src/thread_name.cpp +++ b/libs/system/src/thread_name.cpp @@ -29,7 +29,9 @@ #include #endif -static void SetThreadName(std::thread& thread, const char* threadName) +namespace +{ +void SetThreadName(std::thread& thread, const char* threadName) { #if !MRPT_IN_EMSCRIPTEN auto handle = thread.native_handle(); @@ -37,7 +39,7 @@ static void SetThreadName(std::thread& thread, const char* threadName) #endif } -static std::string GetThreadName(std::thread& thread) +std::string GetThreadName(std::thread& thread) { #if !MRPT_IN_EMSCRIPTEN auto handle = thread.native_handle(); @@ -50,13 +52,13 @@ static std::string GetThreadName(std::thread& thread) #endif } -static void SetThreadName(const char* threadName) +void SetThreadName(const char* threadName) { #if !MRPT_IN_EMSCRIPTEN prctl(PR_SET_NAME, threadName, 0L, 0L, 0L); #endif } -static std::string GetThreadName() +std::string GetThreadName() { #if !MRPT_IN_EMSCRIPTEN char buf[100] = {0}; @@ -66,7 +68,8 @@ static std::string GetThreadName() return {}; #endif } -#endif +} // namespace +#endif // Linux void mrpt::system::thread_name(const std::string& name) { diff --git a/libs/tfest/src/se2_l2.cpp b/libs/tfest/src/se2_l2.cpp index 57fda1086c..a6577b51a1 100644 --- a/libs/tfest/src/se2_l2.cpp +++ b/libs/tfest/src/se2_l2.cpp @@ -36,7 +36,9 @@ bool tfest::se2_l2( } // Non-vectorized version -static mrpt::tfest::internal::se2_l2_impl_return_t se2_l2_impl( +namespace +{ +mrpt::tfest::internal::se2_l2_impl_return_t se2_l2_impl( const TMatchingPairList& in_correspondences) { // SSE vectorized version: @@ -81,6 +83,7 @@ static mrpt::tfest::internal::se2_l2_impl_return_t se2_l2_impl( return ret; } +} // namespace /*--------------------------------------------------------------- leastSquareErrorRigidTransformation diff --git a/libs/tfest/src/se3_l2.cpp b/libs/tfest/src/se3_l2.cpp index 03bac57ec3..bd5e630da9 100644 --- a/libs/tfest/src/se3_l2.cpp +++ b/libs/tfest/src/se3_l2.cpp @@ -67,7 +67,9 @@ using namespace std; /*--------------------------------------------------------------- se3_l2 (old "HornMethod()") ---------------------------------------------------------------*/ -static bool se3_l2_internal( +namespace +{ +bool se3_l2_internal( std::vector& points_this, // IN/OUT: It gets modified! std::vector& @@ -201,6 +203,7 @@ static bool se3_l2_internal( MRPT_END } // end se3_l2_internal() +} // namespace bool tfest::se3_l2( const std::vector& in_points_this, From 5942cf8e60747f9f7f1774a9c5e08800fe44226b Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 6 Nov 2023 10:25:50 +0100 Subject: [PATCH 21/36] TMetricMapInitializer now uses `shared_ptr` --- .../gui/configWidget/CConfigWidget.cpp | 6 +-- doc/source/doxygen-docs/changelog.md | 1 + libs/maps/src/maps/CBeaconMap.cpp | 4 +- libs/maps/src/maps/CColouredOctoMap.cpp | 4 +- libs/maps/src/maps/CColouredPointsMap.cpp | 7 +-- .../src/maps/CGasConcentrationGridMap2D.cpp | 4 +- libs/maps/src/maps/CHeightGridMap2D.cpp | 4 +- libs/maps/src/maps/CHeightGridMap2D_MRF.cpp | 7 +-- libs/maps/src/maps/CMultiMetricMap.cpp | 2 +- .../src/maps/COccupancyGridMap2D_common.cpp | 7 +-- libs/maps/src/maps/COccupancyGridMap3D.cpp | 7 +-- libs/maps/src/maps/COctoMap.cpp | 4 +- libs/maps/src/maps/CPointsMapXYZI.cpp | 4 +- libs/maps/src/maps/CReflectivityGridMap2D.cpp | 4 +- libs/maps/src/maps/CSimplePointsMap.cpp | 4 +- libs/maps/src/maps/CVoxelMap.cpp | 4 +- libs/maps/src/maps/CVoxelMapRGB.cpp | 4 +- libs/maps/src/maps/CWeightedPointsMap.cpp | 7 +-- .../maps/src/maps/CWirelessPowerGridMap2D.cpp | 4 +- .../include/mrpt/maps/TMetricMapInitializer.h | 4 +- .../mrpt/maps/TMetricMapTypesRegistry.h | 54 +++++++++++++------ libs/obs/src/TMetricMapInitializer.cpp | 9 ++-- libs/obs/src/TMetricMapTypesRegistry.cpp | 20 +++---- .../poses/include/mrpt/poses/CPose3DQuatPDF.h | 2 +- libs/poses/src/CPose3DQuatPDF.cpp | 4 +- libs/rtti/src/rtti_unittest.cpp | 2 +- .../include/mrpt/serialization/CArchive.h | 2 +- libs/vision/src/maps/CLandmarksMap.cpp | 4 +- python/src/mrpt/maps/CBeacon.cpp | 6 +-- python/src/mrpt/maps/CColouredOctoMap.cpp | 12 ++--- .../mrpt/maps/CGasConcentrationGridMap2D.cpp | 6 +-- .../src/mrpt/maps/CHeightGridMap2D_Base.cpp | 6 +-- python/src/mrpt/maps/CHeightGridMap2D_MRF.cpp | 6 +-- python/src/mrpt/maps/CLandmarksMap.cpp | 6 +-- python/src/mrpt/maps/COccupancyGridMap2D.cpp | 6 +-- python/src/mrpt/maps/COccupancyGridMap3D.cpp | 6 +-- python/src/mrpt/maps/COctoMap.cpp | 12 ++--- .../mrpt/maps/CPointCloudFilterByDistance.cpp | 6 +-- .../src/mrpt/maps/CReflectivityGridMap2D.cpp | 12 ++--- python/src/mrpt/maps/CVoxelMap.cpp | 12 ++--- .../src/mrpt/maps/CWirelessPowerGridMap2D.cpp | 6 +-- python/src/mrpt/nav/planners/TMoveTree.cpp | 2 - .../reactive/CReactiveNavigationSystem3D.cpp | 2 - python/src/mrpt/poses/CPose3DPDFSOG.cpp | 2 +- 44 files changed, 160 insertions(+), 137 deletions(-) diff --git a/apps/robot-map-gui/gui/configWidget/CConfigWidget.cpp b/apps/robot-map-gui/gui/configWidget/CConfigWidget.cpp index 256e150f50..67cbb40f93 100644 --- a/apps/robot-map-gui/gui/configWidget/CConfigWidget.cpp +++ b/apps/robot-map-gui/gui/configWidget/CConfigWidget.cpp @@ -211,11 +211,11 @@ TSetOfMetricMapInitializers CConfigWidget::config() for (auto& map : it.second) { const std::string sMapName = map->getName().toStdString(); - TMetricMapInitializer* mi = mmr.factoryMapDefinition(sMapName); + auto mi = mmr.factoryMapDefinition(sMapName); ASSERT_(mi); - map->updateConfiguration(mi); - mapCfg.push_back(TMetricMapInitializer::Ptr(mi)); + map->updateConfiguration(mi.get()); + mapCfg.push_back(mi); ++index; } } diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 5c6ecd830a..66566c450e 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -10,6 +10,7 @@ - mrpt::maps::COccupancyGridMap2D - mrpt::maps::COccupancyGridMap2D - New virtual method mrpt::maps::CMetricMap::boundingBox() + - mrpt::maps::TMetricMapInitializer now returns `shared_ptr`s instead of plain pointers. - \ref mrpt_core_grp - Add the `[[nodiscard]]` attribute to all functions returning a value in `` diff --git a/libs/maps/src/maps/CBeaconMap.cpp b/libs/maps/src/maps/CBeaconMap.cpp index ca1c97db2d..3d7070f010 100644 --- a/libs/maps/src/maps/CBeaconMap.cpp +++ b/libs/maps/src/maps/CBeaconMap.cpp @@ -69,12 +69,12 @@ void CBeaconMap::TMapDefinition::dumpToTextStream_map_specific( this->likelihoodOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* CBeaconMap::internal_CreateFromMapDefinition( +mrpt::maps::CMetricMap::Ptr CBeaconMap::internal_CreateFromMapDefinition( const mrpt::maps::TMetricMapInitializer& _def) { const CBeaconMap::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new CBeaconMap(); + auto obj = CBeaconMap::Create(); obj->insertionOptions = def.insertionOpts; obj->likelihoodOptions = def.likelihoodOpts; return obj; diff --git a/libs/maps/src/maps/CColouredOctoMap.cpp b/libs/maps/src/maps/CColouredOctoMap.cpp index cf9e126176..21167dc569 100644 --- a/libs/maps/src/maps/CColouredOctoMap.cpp +++ b/libs/maps/src/maps/CColouredOctoMap.cpp @@ -68,12 +68,12 @@ void CColouredOctoMap::TMapDefinition::dumpToTextStream_map_specific( this->likelihoodOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* CColouredOctoMap::internal_CreateFromMapDefinition( +mrpt::maps::CMetricMap::Ptr CColouredOctoMap::internal_CreateFromMapDefinition( const mrpt::maps::TMetricMapInitializer& _def) { const CColouredOctoMap::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new CColouredOctoMap(def.resolution); + auto obj = CColouredOctoMap::Create(def.resolution); obj->insertionOptions = def.insertionOpts; obj->likelihoodOptions = def.likelihoodOpts; return obj; diff --git a/libs/maps/src/maps/CColouredPointsMap.cpp b/libs/maps/src/maps/CColouredPointsMap.cpp index c2a0ab3a57..1a2f8f4279 100644 --- a/libs/maps/src/maps/CColouredPointsMap.cpp +++ b/libs/maps/src/maps/CColouredPointsMap.cpp @@ -55,12 +55,13 @@ void CColouredPointsMap::TMapDefinition::dumpToTextStream_map_specific( this->colourOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* CColouredPointsMap::internal_CreateFromMapDefinition( - const mrpt::maps::TMetricMapInitializer& _def) +mrpt::maps::CMetricMap::Ptr + CColouredPointsMap::internal_CreateFromMapDefinition( + const mrpt::maps::TMetricMapInitializer& _def) { const CColouredPointsMap::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new CColouredPointsMap(); + auto obj = CColouredPointsMap::Create(); obj->insertionOptions = def.insertionOpts; obj->likelihoodOptions = def.likelihoodOpts; obj->colorScheme = def.colourOpts; diff --git a/libs/maps/src/maps/CGasConcentrationGridMap2D.cpp b/libs/maps/src/maps/CGasConcentrationGridMap2D.cpp index 74d5e3753b..29619ef3c3 100644 --- a/libs/maps/src/maps/CGasConcentrationGridMap2D.cpp +++ b/libs/maps/src/maps/CGasConcentrationGridMap2D.cpp @@ -80,13 +80,13 @@ void CGasConcentrationGridMap2D::TMapDefinition::dumpToTextStream_map_specific( this->insertionOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* +mrpt::maps::CMetricMap::Ptr CGasConcentrationGridMap2D::internal_CreateFromMapDefinition( const mrpt::maps::TMetricMapInitializer& _def) { const CGasConcentrationGridMap2D::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new CGasConcentrationGridMap2D( + auto obj = CGasConcentrationGridMap2D::Create( def.mapType, def.min_x, def.max_x, def.min_y, def.max_y, def.resolution); obj->insertionOptions = def.insertionOpts; diff --git a/libs/maps/src/maps/CHeightGridMap2D.cpp b/libs/maps/src/maps/CHeightGridMap2D.cpp index 0534e04c51..c9212cb793 100644 --- a/libs/maps/src/maps/CHeightGridMap2D.cpp +++ b/libs/maps/src/maps/CHeightGridMap2D.cpp @@ -70,12 +70,12 @@ void CHeightGridMap2D::TMapDefinition::dumpToTextStream_map_specific( this->insertionOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* CHeightGridMap2D::internal_CreateFromMapDefinition( +mrpt::maps::CMetricMap::Ptr CHeightGridMap2D::internal_CreateFromMapDefinition( const mrpt::maps::TMetricMapInitializer& _def) { const CHeightGridMap2D::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new CHeightGridMap2D( + auto obj = CHeightGridMap2D::Create( def.mapType, def.min_x, def.max_x, def.min_y, def.max_y, def.resolution); obj->insertionOptions = def.insertionOpts; diff --git a/libs/maps/src/maps/CHeightGridMap2D_MRF.cpp b/libs/maps/src/maps/CHeightGridMap2D_MRF.cpp index ad208dcc60..da72444657 100644 --- a/libs/maps/src/maps/CHeightGridMap2D_MRF.cpp +++ b/libs/maps/src/maps/CHeightGridMap2D_MRF.cpp @@ -66,12 +66,13 @@ void CHeightGridMap2D_MRF::TMapDefinition::dumpToTextStream_map_specific( this->insertionOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* CHeightGridMap2D_MRF::internal_CreateFromMapDefinition( - const mrpt::maps::TMetricMapInitializer& _def) +mrpt::maps::CMetricMap::Ptr + CHeightGridMap2D_MRF::internal_CreateFromMapDefinition( + const mrpt::maps::TMetricMapInitializer& _def) { const CHeightGridMap2D_MRF::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new CHeightGridMap2D_MRF( + auto obj = CHeightGridMap2D_MRF::Create( def.mapType, def.min_x, def.max_x, def.min_y, def.max_y, def.resolution, def.run_map_estimation_at_ctor); obj->insertionOptions = def.insertionOpts; diff --git a/libs/maps/src/maps/CMultiMetricMap.cpp b/libs/maps/src/maps/CMultiMetricMap.cpp index 24ee56c873..da7d0433aa 100644 --- a/libs/maps/src/maps/CMultiMetricMap.cpp +++ b/libs/maps/src/maps/CMultiMetricMap.cpp @@ -123,7 +123,7 @@ void CMultiMetricMap::setListOfMaps(const TSetOfMetricMapInitializers& inits) for (const auto& i : inits) { // Create map from the list of all params: - auto* theMap = mmr.factoryMapObjectFromDefinition(*i.get()); + auto theMap = mmr.factoryMapObjectFromDefinition(*i.get()); ASSERT_(theMap); // Add to the list of maps: this->maps.emplace_back(theMap); diff --git a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp index 185358bc0f..6ef5ae0cc4 100644 --- a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp +++ b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp @@ -69,12 +69,13 @@ void COccupancyGridMap2D::TMapDefinition::dumpToTextStream_map_specific( this->likelihoodOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* COccupancyGridMap2D::internal_CreateFromMapDefinition( - const mrpt::maps::TMetricMapInitializer& _def) +mrpt::maps::CMetricMap::Ptr + COccupancyGridMap2D::internal_CreateFromMapDefinition( + const mrpt::maps::TMetricMapInitializer& _def) { const COccupancyGridMap2D::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new COccupancyGridMap2D( + auto obj = COccupancyGridMap2D::Create( def.min_x, def.max_x, def.min_y, def.max_y, def.resolution); obj->insertionOptions = def.insertionOpts; obj->likelihoodOptions = def.likelihoodOpts; diff --git a/libs/maps/src/maps/COccupancyGridMap3D.cpp b/libs/maps/src/maps/COccupancyGridMap3D.cpp index 88af2bced9..8f32de28bc 100644 --- a/libs/maps/src/maps/COccupancyGridMap3D.cpp +++ b/libs/maps/src/maps/COccupancyGridMap3D.cpp @@ -63,11 +63,12 @@ void COccupancyGridMap3D::TMapDefinition::dumpToTextStream_map_specific( this->likelihoodOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* COccupancyGridMap3D::internal_CreateFromMapDefinition( - const mrpt::maps::TMetricMapInitializer& _def) +mrpt::maps::CMetricMap::Ptr + COccupancyGridMap3D::internal_CreateFromMapDefinition( + const mrpt::maps::TMetricMapInitializer& _def) { auto& def = dynamic_cast(_def); - auto* obj = new COccupancyGridMap3D( + auto obj = COccupancyGridMap3D::Create( mrpt::math::TPoint3D(def.min_x, def.min_y, def.min_z), mrpt::math::TPoint3D(def.max_x, def.max_y, def.max_z), def.resolution); obj->insertionOptions = def.insertionOpts; diff --git a/libs/maps/src/maps/COctoMap.cpp b/libs/maps/src/maps/COctoMap.cpp index 6ce25f846a..d86f6de1f5 100644 --- a/libs/maps/src/maps/COctoMap.cpp +++ b/libs/maps/src/maps/COctoMap.cpp @@ -60,12 +60,12 @@ void COctoMap::TMapDefinition::dumpToTextStream_map_specific( this->likelihoodOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* COctoMap::internal_CreateFromMapDefinition( +mrpt::maps::CMetricMap::Ptr COctoMap::internal_CreateFromMapDefinition( const mrpt::maps::TMetricMapInitializer& _def) { const COctoMap::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new COctoMap(def.resolution); + auto obj = COctoMap::Create(def.resolution); obj->insertionOptions = def.insertionOpts; obj->likelihoodOptions = def.likelihoodOpts; return obj; diff --git a/libs/maps/src/maps/CPointsMapXYZI.cpp b/libs/maps/src/maps/CPointsMapXYZI.cpp index 47212a6273..ad18ee6321 100644 --- a/libs/maps/src/maps/CPointsMapXYZI.cpp +++ b/libs/maps/src/maps/CPointsMapXYZI.cpp @@ -56,12 +56,12 @@ void CPointsMapXYZI::TMapDefinition::dumpToTextStream_map_specific( this->likelihoodOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* CPointsMapXYZI::internal_CreateFromMapDefinition( +mrpt::maps::CMetricMap::Ptr CPointsMapXYZI::internal_CreateFromMapDefinition( const mrpt::maps::TMetricMapInitializer& _def) { const CPointsMapXYZI::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new CPointsMapXYZI(); + auto obj = CPointsMapXYZI::Create(); obj->insertionOptions = def.insertionOpts; obj->likelihoodOptions = def.likelihoodOpts; return obj; diff --git a/libs/maps/src/maps/CReflectivityGridMap2D.cpp b/libs/maps/src/maps/CReflectivityGridMap2D.cpp index 81154e3b7c..315c50ac09 100644 --- a/libs/maps/src/maps/CReflectivityGridMap2D.cpp +++ b/libs/maps/src/maps/CReflectivityGridMap2D.cpp @@ -65,13 +65,13 @@ void CReflectivityGridMap2D::TMapDefinition::dumpToTextStream_map_specific( this->insertionOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* +mrpt::maps::CMetricMap::Ptr CReflectivityGridMap2D::internal_CreateFromMapDefinition( const mrpt::maps::TMetricMapInitializer& _def) { const CReflectivityGridMap2D::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new CReflectivityGridMap2D( + auto obj = CReflectivityGridMap2D::Create( def.min_x, def.max_x, def.min_y, def.max_y, def.resolution); obj->insertionOptions = def.insertionOpts; return obj; diff --git a/libs/maps/src/maps/CSimplePointsMap.cpp b/libs/maps/src/maps/CSimplePointsMap.cpp index 38ed822245..6f213c80b8 100644 --- a/libs/maps/src/maps/CSimplePointsMap.cpp +++ b/libs/maps/src/maps/CSimplePointsMap.cpp @@ -43,12 +43,12 @@ void CSimplePointsMap::TMapDefinition::dumpToTextStream_map_specific( this->renderOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* CSimplePointsMap::internal_CreateFromMapDefinition( +mrpt::maps::CMetricMap::Ptr CSimplePointsMap::internal_CreateFromMapDefinition( const mrpt::maps::TMetricMapInitializer& _def) { const CSimplePointsMap::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new CSimplePointsMap(); + auto obj = CSimplePointsMap::Create(); obj->insertionOptions = def.insertionOpts; obj->likelihoodOptions = def.likelihoodOpts; obj->renderOptions = def.renderOpts; diff --git a/libs/maps/src/maps/CVoxelMap.cpp b/libs/maps/src/maps/CVoxelMap.cpp index dadea25917..6472c359ff 100644 --- a/libs/maps/src/maps/CVoxelMap.cpp +++ b/libs/maps/src/maps/CVoxelMap.cpp @@ -45,12 +45,12 @@ void CVoxelMap::TMapDefinition::dumpToTextStream_map_specific( this->likelihoodOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* CVoxelMap::internal_CreateFromMapDefinition( +mrpt::maps::CMetricMap::Ptr CVoxelMap::internal_CreateFromMapDefinition( const mrpt::maps::TMetricMapInitializer& _def) { const CVoxelMap::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new CVoxelMap(def.resolution); + auto obj = CVoxelMap::Create(def.resolution); obj->insertionOptions = def.insertionOpts; obj->likelihoodOptions = def.likelihoodOpts; return obj; diff --git a/libs/maps/src/maps/CVoxelMapRGB.cpp b/libs/maps/src/maps/CVoxelMapRGB.cpp index 7b65b584c5..95ad5c9871 100644 --- a/libs/maps/src/maps/CVoxelMapRGB.cpp +++ b/libs/maps/src/maps/CVoxelMapRGB.cpp @@ -47,12 +47,12 @@ void CVoxelMapRGB::TMapDefinition::dumpToTextStream_map_specific( this->likelihoodOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* CVoxelMapRGB::internal_CreateFromMapDefinition( +mrpt::maps::CMetricMap::Ptr CVoxelMapRGB::internal_CreateFromMapDefinition( const mrpt::maps::TMetricMapInitializer& _def) { const CVoxelMapRGB::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new CVoxelMapRGB(def.resolution); + auto obj = CVoxelMapRGB::Create(def.resolution); obj->insertionOptions = def.insertionOpts; obj->likelihoodOptions = def.likelihoodOpts; return obj; diff --git a/libs/maps/src/maps/CWeightedPointsMap.cpp b/libs/maps/src/maps/CWeightedPointsMap.cpp index a2bb117880..8a8671348c 100644 --- a/libs/maps/src/maps/CWeightedPointsMap.cpp +++ b/libs/maps/src/maps/CWeightedPointsMap.cpp @@ -45,12 +45,13 @@ void CWeightedPointsMap::TMapDefinition::dumpToTextStream_map_specific( this->likelihoodOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* CWeightedPointsMap::internal_CreateFromMapDefinition( - const mrpt::maps::TMetricMapInitializer& _def) +mrpt::maps::CMetricMap::Ptr + CWeightedPointsMap::internal_CreateFromMapDefinition( + const mrpt::maps::TMetricMapInitializer& _def) { const CWeightedPointsMap::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new CWeightedPointsMap(); + auto obj = CWeightedPointsMap::Create(); obj->insertionOptions = def.insertionOpts; obj->likelihoodOptions = def.likelihoodOpts; return obj; diff --git a/libs/maps/src/maps/CWirelessPowerGridMap2D.cpp b/libs/maps/src/maps/CWirelessPowerGridMap2D.cpp index 7fb1160506..017015fe50 100644 --- a/libs/maps/src/maps/CWirelessPowerGridMap2D.cpp +++ b/libs/maps/src/maps/CWirelessPowerGridMap2D.cpp @@ -67,13 +67,13 @@ void CWirelessPowerGridMap2D::TMapDefinition::dumpToTextStream_map_specific( this->insertionOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* +mrpt::maps::CMetricMap::Ptr CWirelessPowerGridMap2D::internal_CreateFromMapDefinition( const mrpt::maps::TMetricMapInitializer& _def) { const CWirelessPowerGridMap2D::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new CWirelessPowerGridMap2D( + auto obj = CWirelessPowerGridMap2D::Create( def.mapType, def.min_x, def.max_x, def.min_y, def.max_y, def.resolution); obj->insertionOptions = def.insertionOpts; diff --git a/libs/obs/include/mrpt/maps/TMetricMapInitializer.h b/libs/obs/include/mrpt/maps/TMetricMapInitializer.h index 77c2e18364..bd03f10aa6 100644 --- a/libs/obs/include/mrpt/maps/TMetricMapInitializer.h +++ b/libs/obs/include/mrpt/maps/TMetricMapInitializer.h @@ -64,7 +64,7 @@ struct TMetricMapInitializer : public mrpt::config::CLoadableOptions /** Looks up in the registry of known map types and call the corresponding * `::MapDefinition()`. */ - static TMetricMapInitializer* factory(const std::string& mapClassName); + static Ptr factory(const std::string& mapClassName); protected: TMetricMapInitializer(const mrpt::rtti::TRuntimeClassId* classID); @@ -97,7 +97,7 @@ class TSetOfMetricMapInitializers : public mrpt::config::CLoadableOptions template void push_back(const MAP_DEFINITION& o) { - m_list.push_back(TMetricMapInitializer::Ptr(new MAP_DEFINITION(o))); + m_list.push_back(std::make_shared(o)); } void push_back(const TMetricMapInitializer::Ptr& o) { m_list.push_back(o); } diff --git a/libs/obs/include/mrpt/maps/TMetricMapTypesRegistry.h b/libs/obs/include/mrpt/maps/TMetricMapTypesRegistry.h index 890476b845..8be37572db 100644 --- a/libs/obs/include/mrpt/maps/TMetricMapTypesRegistry.h +++ b/libs/obs/include/mrpt/maps/TMetricMapTypesRegistry.h @@ -13,6 +13,7 @@ #include #include +#include #include namespace mrpt @@ -23,9 +24,10 @@ struct TMetricMapInitializer; namespace internal { using MapDefCtorFunctor = - std::function; -using MapCtorFromDefFunctor = std::function; + std::function(void)>; +using MapCtorFromDefFunctor = + std::function( + const mrpt::maps::TMetricMapInitializer&)>; /** Class factory & registry for map classes. Used from * mrpt::maps::TMetricMapInitializer */ @@ -38,14 +40,30 @@ struct TMetricMapTypesRegistry size_t doRegister( const std::string& name, MapDefCtorFunctor func1, MapCtorFromDefFunctor func2); + /** Return nullptr if not found */ - mrpt::maps::TMetricMapInitializer* factoryMapDefinition( + std::shared_ptr factoryMapDefinition( const std::string& className) const; + /** Return nullptr if not found */ - mrpt::maps::CMetricMap* factoryMapObjectFromDefinition( + std::shared_ptr factoryMapObjectFromDefinition( const mrpt::maps::TMetricMapInitializer& mi) const; - using TListRegisteredMaps = std::map< - std::string, std::pair>; + + struct InfoPerMapClass + { + InfoPerMapClass() = default; + InfoPerMapClass( + const MapDefCtorFunctor& DefCtor, + const MapCtorFromDefFunctor& MapCtor) + : defCtor(DefCtor), mapCtor(MapCtor) + { + } + + MapDefCtorFunctor defCtor; + MapCtorFromDefFunctor mapCtor; + }; + + using TListRegisteredMaps = std::map; const TListRegisteredMaps& getAllRegistered() const { return m_registry; } private: @@ -78,13 +96,14 @@ struct TMetricMapTypesRegistry ; \ /** Returns default map definition initializer. See \ * mrpt::maps::TMetricMapInitializer */ \ - static mrpt::maps::TMetricMapInitializer* MapDefinition(); \ + static std::shared_ptr MapDefinition(); \ /** Constructor from a map definition structure: initializes the map and \ * its parameters accordingly */ \ - static _CLASS_NAME_* CreateFromMapDefinition( \ - const mrpt::maps::TMetricMapInitializer& def); \ - static mrpt::maps::CMetricMap* internal_CreateFromMapDefinition( \ + static std::shared_ptr<_CLASS_NAME_> CreateFromMapDefinition( \ const mrpt::maps::TMetricMapInitializer& def); \ + static std::shared_ptr \ + internal_CreateFromMapDefinition( \ + const mrpt::maps::TMetricMapInitializer& def); \ /** ID used to initialize class registration (just ignore it) */ \ static const size_t m_private_map_register_id; \ /** @} */ @@ -97,17 +116,18 @@ struct TMetricMapTypesRegistry mrpt::maps::internal::TMetricMapTypesRegistry::Instance().doRegister( \ _CLASSNAME_STRINGS, &_CLASSNAME_WITH_NS::MapDefinition, \ &_CLASSNAME_WITH_NS::internal_CreateFromMapDefinition); \ - mrpt::maps::TMetricMapInitializer* _CLASSNAME_WITH_NS::MapDefinition() \ + std::shared_ptr \ + _CLASSNAME_WITH_NS::MapDefinition() \ { \ - return new _CLASSNAME_WITH_NS::TMapDefinition; \ + return std::make_shared<_CLASSNAME_WITH_NS::TMapDefinition>(); \ } \ - _CLASSNAME_WITH_NS* _CLASSNAME_WITH_NS::CreateFromMapDefinition( \ - const mrpt::maps::TMetricMapInitializer& def) \ + std::shared_ptr<_CLASSNAME_WITH_NS> \ + _CLASSNAME_WITH_NS::CreateFromMapDefinition( \ + const mrpt::maps::TMetricMapInitializer& def) \ { \ - return dynamic_cast<_CLASSNAME_WITH_NS*>( \ + return std::dynamic_pointer_cast<_CLASSNAME_WITH_NS>( \ _CLASSNAME_WITH_NS::internal_CreateFromMapDefinition(def)); \ } - } // namespace internal } // namespace maps } // namespace mrpt diff --git a/libs/obs/src/TMetricMapInitializer.cpp b/libs/obs/src/TMetricMapInitializer.cpp index 7789874e49..b0a662664c 100644 --- a/libs/obs/src/TMetricMapInitializer.cpp +++ b/libs/obs/src/TMetricMapInitializer.cpp @@ -19,7 +19,7 @@ using namespace mrpt::maps; /** Looks up in the registry of known map types and call the corresponding * `::MapDefinition()`. */ -TMetricMapInitializer* TMetricMapInitializer::factory( +TMetricMapInitializer::Ptr TMetricMapInitializer::factory( const std::string& mapClassName) { using mrpt::maps::internal::TMetricMapTypesRegistry; @@ -72,6 +72,7 @@ void TSetOfMetricMapInitializers::loadFromConfigFile( MRPT_START using mrpt::maps::internal::TMetricMapTypesRegistry; + using namespace std::string_literals; // Delete previous contents: clear(); @@ -86,10 +87,10 @@ void TSetOfMetricMapInitializers::loadFromConfigFile( const std::string sMapName = allMapKind.first; unsigned int n = - ini.read_uint64_t(sectionName, sMapName + string("_count"), 0); + ini.read_uint64_t(sectionName, sMapName + "_count"s, 0); for (unsigned int i = 0; i < n; i++) { - TMetricMapInitializer* mi = mmr.factoryMapDefinition(sMapName); + TMetricMapInitializer::Ptr mi = mmr.factoryMapDefinition(sMapName); ASSERT_(mi); // Load from sections formatted like this: @@ -103,7 +104,7 @@ void TSetOfMetricMapInitializers::loadFromConfigFile( mi->loadFromConfigFile(ini, sMapSectionsPrefix); // Add the params to the list: - this->push_back(TMetricMapInitializer::Ptr(mi)); + this->push_back(mi); } } // end for each map kind diff --git a/libs/obs/src/TMetricMapTypesRegistry.cpp b/libs/obs/src/TMetricMapTypesRegistry.cpp index bff5a65244..f4604a018b 100644 --- a/libs/obs/src/TMetricMapTypesRegistry.cpp +++ b/libs/obs/src/TMetricMapTypesRegistry.cpp @@ -45,16 +45,15 @@ size_t TMetricMapTypesRegistry::doRegister( mrpt::system::tokenize(names, " \t\r\n,", lstNames); for (const auto& lstName : lstNames) { - const auto p = std::make_pair(func1, func2); - m_registry[lstName] = p; + m_registry[lstName] = {func1, func2}; // register also the version without the "mrpt::NS::" prefix, for // backwards compatibility: - m_registry[stripNamespace(lstName)] = p; + m_registry[stripNamespace(lstName)] = {func1, func2}; } return m_registry.size(); } -mrpt::maps::TMetricMapInitializer* +mrpt::maps::TMetricMapInitializer::Ptr TMetricMapTypesRegistry::factoryMapDefinition( const std::string& className) const { @@ -64,12 +63,13 @@ mrpt::maps::TMetricMapInitializer* if (it == m_registry.end()) it = m_registry.find(stripNamespace(className)); if (it == m_registry.end()) return nullptr; - ASSERT_(it->second.first); - return (it->second.first)(); + ASSERT_(it->second.defCtor); + return (it->second.defCtor)(); } -mrpt::maps::CMetricMap* TMetricMapTypesRegistry::factoryMapObjectFromDefinition( - const mrpt::maps::TMetricMapInitializer& mi) const +mrpt::maps::CMetricMap::Ptr + TMetricMapTypesRegistry::factoryMapObjectFromDefinition( + const mrpt::maps::TMetricMapInitializer& mi) const { auto it = m_registry.find(mi.getMetricMapClassType()->className); if (it == m_registry.end()) @@ -80,8 +80,8 @@ mrpt::maps::CMetricMap* TMetricMapTypesRegistry::factoryMapObjectFromDefinition( mi.getMetricMapClassType()->className); } - ASSERT_(it->second.second); - mrpt::maps::CMetricMap* theMap = (it->second.second)(mi); + ASSERT_(it->second.mapCtor); + mrpt::maps::CMetricMap::Ptr theMap = (it->second.mapCtor)(mi); // Common params for all maps: theMap->genericMapParams = mi.genericMapParams; diff --git a/libs/poses/include/mrpt/poses/CPose3DQuatPDF.h b/libs/poses/include/mrpt/poses/CPose3DQuatPDF.h index 6a76f6011d..ca3c4572e2 100644 --- a/libs/poses/include/mrpt/poses/CPose3DQuatPDF.h +++ b/libs/poses/include/mrpt/poses/CPose3DQuatPDF.h @@ -59,7 +59,7 @@ class CPose3DQuatPDF * anymore. * \sa copyFrom */ - static CPose3DQuatPDF* createFrom2D(const CPosePDF& o); + static CPose3DQuatPDF::Ptr createFrom2D(const CPosePDF& o); /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF */ virtual void inverse(CPose3DQuatPDF& o) const = 0; diff --git a/libs/poses/src/CPose3DQuatPDF.cpp b/libs/poses/src/CPose3DQuatPDF.cpp index cafddb5c27..2caa18789d 100644 --- a/libs/poses/src/CPose3DQuatPDF.cpp +++ b/libs/poses/src/CPose3DQuatPDF.cpp @@ -25,14 +25,14 @@ IMPLEMENTS_VIRTUAL_SERIALIZABLE(CPose3DQuatPDF, CSerializable, mrpt::poses) /*--------------------------------------------------------------- copyFrom2D ---------------------------------------------------------------*/ -CPose3DQuatPDF* CPose3DQuatPDF::createFrom2D(const CPosePDF& o) +CPose3DQuatPDF::Ptr CPose3DQuatPDF::createFrom2D(const CPosePDF& o) { MRPT_START CPose3DPDFGaussian q; q.copyFrom(o); - return new CPose3DQuatPDFGaussian(q); + return std::make_shared(q); MRPT_END } diff --git a/libs/rtti/src/rtti_unittest.cpp b/libs/rtti/src/rtti_unittest.cpp index d31b040b62..4e67237407 100644 --- a/libs/rtti/src/rtti_unittest.cpp +++ b/libs/rtti/src/rtti_unittest.cpp @@ -64,7 +64,7 @@ TEST(rtti, MyDerived1_CLASSID) // RTTI IS_DERIVED(*) { - auto p = mrpt::rtti::CObject::Ptr(new MyNS::MyDerived1); + auto p = mrpt::rtti::CObject::Ptr(std::make_shared()); EXPECT_TRUE(IS_DERIVED(*p, MyNS::MyDerived1)); EXPECT_TRUE(IS_DERIVED(*p, mrpt::rtti::CObject)); } diff --git a/libs/serialization/include/mrpt/serialization/CArchive.h b/libs/serialization/include/mrpt/serialization/CArchive.h index 31d67dabab..ea4a5863fd 100644 --- a/libs/serialization/include/mrpt/serialization/CArchive.h +++ b/libs/serialization/include/mrpt/serialization/CArchive.h @@ -568,7 +568,7 @@ CArchive& operator>>(CArchive& in, std::shared_ptr& pObj) else { ASSERT_EQUAL_(expected_name, stored_name); - pObj.reset(new T); + pObj = std::make_shared(); in >> *pObj; } return in; diff --git a/libs/vision/src/maps/CLandmarksMap.cpp b/libs/vision/src/maps/CLandmarksMap.cpp index 1943dea086..cf29137000 100644 --- a/libs/vision/src/maps/CLandmarksMap.cpp +++ b/libs/vision/src/maps/CLandmarksMap.cpp @@ -99,12 +99,12 @@ void CLandmarksMap::TMapDefinition::dumpToTextStream_map_specific( this->likelihoodOpts.dumpToTextStream(out); } -mrpt::maps::CMetricMap* CLandmarksMap::internal_CreateFromMapDefinition( +mrpt::maps::CMetricMap::Ptr CLandmarksMap::internal_CreateFromMapDefinition( const mrpt::maps::TMetricMapInitializer& _def) { const CLandmarksMap::TMapDefinition& def = *dynamic_cast(&_def); - auto* obj = new CLandmarksMap(); + auto obj = CLandmarksMap::Create(); for (const auto& initialBeacon : def.initialBeacons) { diff --git a/python/src/mrpt/maps/CBeacon.cpp b/python/src/mrpt/maps/CBeacon.cpp index fe6deb1367..18745ef11b 100644 --- a/python/src/mrpt/maps/CBeacon.cpp +++ b/python/src/mrpt/maps/CBeacon.cpp @@ -585,7 +585,7 @@ struct PyCallBack_mrpt_maps_CBeaconMap_TInsertionOptions : public mrpt::maps::CB } }; -// mrpt::maps::CBeaconMap::TMapDefinition file: line:67 +// mrpt::maps::CBeaconMap::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_CBeaconMap_TMapDefinition : public mrpt::maps::CBeaconMap::TMapDefinition { using mrpt::maps::CBeaconMap::TMapDefinition::TMapDefinition; @@ -737,12 +737,12 @@ void bind_mrpt_maps_CBeacon(std::function< pybind11::module &(std::string const cl.def("assign", (struct mrpt::maps::CBeaconMap::TInsertionOptions & (mrpt::maps::CBeaconMap::TInsertionOptions::*)(const struct mrpt::maps::CBeaconMap::TInsertionOptions &)) &mrpt::maps::CBeaconMap::TInsertionOptions::operator=, "C++: mrpt::maps::CBeaconMap::TInsertionOptions::operator=(const struct mrpt::maps::CBeaconMap::TInsertionOptions &) --> struct mrpt::maps::CBeaconMap::TInsertionOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::CBeaconMap::TMapDefinitionBase file: line:62 + { // mrpt::maps::CBeaconMap::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::CBeaconMap::TMapDefinition file: line:67 + { // mrpt::maps::CBeaconMap::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CBeaconMap_TMapDefinition, mrpt::maps::CBeaconMap::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::CBeaconMap::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CBeaconMap_TMapDefinition(); } ) ); diff --git a/python/src/mrpt/maps/CColouredOctoMap.cpp b/python/src/mrpt/maps/CColouredOctoMap.cpp index 64fcaf132e..69357914cb 100644 --- a/python/src/mrpt/maps/CColouredOctoMap.cpp +++ b/python/src/mrpt/maps/CColouredOctoMap.cpp @@ -534,7 +534,7 @@ struct PyCallBack_mrpt_maps_CColouredOctoMap : public mrpt::maps::CColouredOctoM } }; -// mrpt::maps::CColouredOctoMap::TMapDefinition file: line:67 +// mrpt::maps::CColouredOctoMap::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_CColouredOctoMap_TMapDefinition : public mrpt::maps::CColouredOctoMap::TMapDefinition { using mrpt::maps::CColouredOctoMap::TMapDefinition::TMapDefinition; @@ -1137,7 +1137,7 @@ struct PyCallBack_mrpt_maps_CColouredPointsMap_TColourOptions : public mrpt::map } }; -// mrpt::maps::CColouredPointsMap::TMapDefinition file: line:67 +// mrpt::maps::CColouredPointsMap::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_CColouredPointsMap_TMapDefinition : public mrpt::maps::CColouredPointsMap::TMapDefinition { using mrpt::maps::CColouredPointsMap::TMapDefinition::TMapDefinition; @@ -1240,12 +1240,12 @@ void bind_mrpt_maps_CColouredOctoMap(std::function< pybind11::module &(std::stri cl.def("getClampingThresMaxLog", (float (mrpt::maps::CColouredOctoMap::*)() const) &mrpt::maps::CColouredOctoMap::getClampingThresMaxLog, "C++: mrpt::maps::CColouredOctoMap::getClampingThresMaxLog() const --> float"); cl.def("assign", (class mrpt::maps::CColouredOctoMap & (mrpt::maps::CColouredOctoMap::*)(const class mrpt::maps::CColouredOctoMap &)) &mrpt::maps::CColouredOctoMap::operator=, "C++: mrpt::maps::CColouredOctoMap::operator=(const class mrpt::maps::CColouredOctoMap &) --> class mrpt::maps::CColouredOctoMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); - { // mrpt::maps::CColouredOctoMap::TMapDefinitionBase file: line:62 + { // mrpt::maps::CColouredOctoMap::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::CColouredOctoMap::TMapDefinition file: line:67 + { // mrpt::maps::CColouredOctoMap::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CColouredOctoMap_TMapDefinition, mrpt::maps::CColouredOctoMap::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::CColouredOctoMap::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CColouredOctoMap_TMapDefinition(); } ) ); @@ -1312,12 +1312,12 @@ void bind_mrpt_maps_CColouredOctoMap(std::function< pybind11::module &(std::stri cl.def("assign", (struct mrpt::maps::CColouredPointsMap::TColourOptions & (mrpt::maps::CColouredPointsMap::TColourOptions::*)(const struct mrpt::maps::CColouredPointsMap::TColourOptions &)) &mrpt::maps::CColouredPointsMap::TColourOptions::operator=, "C++: mrpt::maps::CColouredPointsMap::TColourOptions::operator=(const struct mrpt::maps::CColouredPointsMap::TColourOptions &) --> struct mrpt::maps::CColouredPointsMap::TColourOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::CColouredPointsMap::TMapDefinitionBase file: line:62 + { // mrpt::maps::CColouredPointsMap::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::CColouredPointsMap::TMapDefinition file: line:67 + { // mrpt::maps::CColouredPointsMap::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CColouredPointsMap_TMapDefinition, mrpt::maps::CColouredPointsMap::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::CColouredPointsMap::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CColouredPointsMap_TMapDefinition(); } ) ); diff --git a/python/src/mrpt/maps/CGasConcentrationGridMap2D.cpp b/python/src/mrpt/maps/CGasConcentrationGridMap2D.cpp index 8cb788b707..8c1dfcf1d9 100644 --- a/python/src/mrpt/maps/CGasConcentrationGridMap2D.cpp +++ b/python/src/mrpt/maps/CGasConcentrationGridMap2D.cpp @@ -508,7 +508,7 @@ struct PyCallBack_mrpt_maps_CGasConcentrationGridMap2D_TInsertionOptions : publi } }; -// mrpt::maps::CGasConcentrationGridMap2D::TMapDefinition file: line:67 +// mrpt::maps::CGasConcentrationGridMap2D::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_CGasConcentrationGridMap2D_TMapDefinition : public mrpt::maps::CGasConcentrationGridMap2D::TMapDefinition { using mrpt::maps::CGasConcentrationGridMap2D::TMapDefinition::TMapDefinition; @@ -625,12 +625,12 @@ void bind_mrpt_maps_CGasConcentrationGridMap2D(std::function< pybind11::module & cl.def("assign", (struct mrpt::maps::CGasConcentrationGridMap2D::TGaussianWindTable & (mrpt::maps::CGasConcentrationGridMap2D::TGaussianWindTable::*)(const struct mrpt::maps::CGasConcentrationGridMap2D::TGaussianWindTable &)) &mrpt::maps::CGasConcentrationGridMap2D::TGaussianWindTable::operator=, "C++: mrpt::maps::CGasConcentrationGridMap2D::TGaussianWindTable::operator=(const struct mrpt::maps::CGasConcentrationGridMap2D::TGaussianWindTable &) --> struct mrpt::maps::CGasConcentrationGridMap2D::TGaussianWindTable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::CGasConcentrationGridMap2D::TMapDefinitionBase file: line:62 + { // mrpt::maps::CGasConcentrationGridMap2D::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::CGasConcentrationGridMap2D::TMapDefinition file: line:67 + { // mrpt::maps::CGasConcentrationGridMap2D::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CGasConcentrationGridMap2D_TMapDefinition, mrpt::maps::CGasConcentrationGridMap2D::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::CGasConcentrationGridMap2D::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CGasConcentrationGridMap2D_TMapDefinition(); } ) ); diff --git a/python/src/mrpt/maps/CHeightGridMap2D_Base.cpp b/python/src/mrpt/maps/CHeightGridMap2D_Base.cpp index b398b02b0c..6b186ccc6f 100644 --- a/python/src/mrpt/maps/CHeightGridMap2D_Base.cpp +++ b/python/src/mrpt/maps/CHeightGridMap2D_Base.cpp @@ -583,7 +583,7 @@ struct PyCallBack_mrpt_maps_CHeightGridMap2D_TInsertionOptions : public mrpt::ma } }; -// mrpt::maps::CHeightGridMap2D::TMapDefinition file: line:67 +// mrpt::maps::CHeightGridMap2D::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_CHeightGridMap2D_TMapDefinition : public mrpt::maps::CHeightGridMap2D::TMapDefinition { using mrpt::maps::CHeightGridMap2D::TMapDefinition::TMapDefinition; @@ -723,12 +723,12 @@ void bind_mrpt_maps_CHeightGridMap2D_Base(std::function< pybind11::module &(std: cl.def("assign", (struct mrpt::maps::CHeightGridMap2D::TInsertionOptions & (mrpt::maps::CHeightGridMap2D::TInsertionOptions::*)(const struct mrpt::maps::CHeightGridMap2D::TInsertionOptions &)) &mrpt::maps::CHeightGridMap2D::TInsertionOptions::operator=, "C++: mrpt::maps::CHeightGridMap2D::TInsertionOptions::operator=(const struct mrpt::maps::CHeightGridMap2D::TInsertionOptions &) --> struct mrpt::maps::CHeightGridMap2D::TInsertionOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::CHeightGridMap2D::TMapDefinitionBase file: line:62 + { // mrpt::maps::CHeightGridMap2D::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::CHeightGridMap2D::TMapDefinition file: line:67 + { // mrpt::maps::CHeightGridMap2D::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CHeightGridMap2D_TMapDefinition, mrpt::maps::CHeightGridMap2D::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::CHeightGridMap2D::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CHeightGridMap2D_TMapDefinition(); } ) ); diff --git a/python/src/mrpt/maps/CHeightGridMap2D_MRF.cpp b/python/src/mrpt/maps/CHeightGridMap2D_MRF.cpp index 12c3dbf64b..9ad25e11dc 100644 --- a/python/src/mrpt/maps/CHeightGridMap2D_MRF.cpp +++ b/python/src/mrpt/maps/CHeightGridMap2D_MRF.cpp @@ -587,7 +587,7 @@ struct PyCallBack_mrpt_maps_CHeightGridMap2D_MRF_TInsertionOptions : public mrpt } }; -// mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition file: line:67 +// mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_CHeightGridMap2D_MRF_TMapDefinition : public mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition { using mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition::TMapDefinition; @@ -677,12 +677,12 @@ void bind_mrpt_maps_CHeightGridMap2D_MRF(std::function< pybind11::module &(std:: cl.def("assign", (struct mrpt::maps::CHeightGridMap2D_MRF::TInsertionOptions & (mrpt::maps::CHeightGridMap2D_MRF::TInsertionOptions::*)(const struct mrpt::maps::CHeightGridMap2D_MRF::TInsertionOptions &)) &mrpt::maps::CHeightGridMap2D_MRF::TInsertionOptions::operator=, "C++: mrpt::maps::CHeightGridMap2D_MRF::TInsertionOptions::operator=(const struct mrpt::maps::CHeightGridMap2D_MRF::TInsertionOptions &) --> struct mrpt::maps::CHeightGridMap2D_MRF::TInsertionOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::CHeightGridMap2D_MRF::TMapDefinitionBase file: line:62 + { // mrpt::maps::CHeightGridMap2D_MRF::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition file: line:67 + { // mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CHeightGridMap2D_MRF_TMapDefinition, mrpt::maps::CHeightGridMap2D_MRF::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::CHeightGridMap2D_MRF::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CHeightGridMap2D_MRF_TMapDefinition(); } ) ); diff --git a/python/src/mrpt/maps/CLandmarksMap.cpp b/python/src/mrpt/maps/CLandmarksMap.cpp index 9bd68adfab..5a5dc8fcb9 100644 --- a/python/src/mrpt/maps/CLandmarksMap.cpp +++ b/python/src/mrpt/maps/CLandmarksMap.cpp @@ -390,7 +390,7 @@ struct PyCallBack_mrpt_maps_CLandmarksMap_TLikelihoodOptions : public mrpt::maps } }; -// mrpt::maps::CLandmarksMap::TMapDefinition file: line:67 +// mrpt::maps::CLandmarksMap::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_CLandmarksMap_TMapDefinition : public mrpt::maps::CLandmarksMap::TMapDefinition { using mrpt::maps::CLandmarksMap::TMapDefinition::TMapDefinition; @@ -583,12 +583,12 @@ void bind_mrpt_maps_CLandmarksMap(std::function< pybind11::module &(std::string cl.def("assign", (struct mrpt::maps::CLandmarksMap::TFuseOptions & (mrpt::maps::CLandmarksMap::TFuseOptions::*)(const struct mrpt::maps::CLandmarksMap::TFuseOptions &)) &mrpt::maps::CLandmarksMap::TFuseOptions::operator=, "C++: mrpt::maps::CLandmarksMap::TFuseOptions::operator=(const struct mrpt::maps::CLandmarksMap::TFuseOptions &) --> struct mrpt::maps::CLandmarksMap::TFuseOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::CLandmarksMap::TMapDefinitionBase file: line:62 + { // mrpt::maps::CLandmarksMap::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::CLandmarksMap::TMapDefinition file: line:67 + { // mrpt::maps::CLandmarksMap::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CLandmarksMap_TMapDefinition, mrpt::maps::CLandmarksMap::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::CLandmarksMap::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CLandmarksMap_TMapDefinition(); } ) ); diff --git a/python/src/mrpt/maps/COccupancyGridMap2D.cpp b/python/src/mrpt/maps/COccupancyGridMap2D.cpp index f9ff15c651..e6e7c25944 100644 --- a/python/src/mrpt/maps/COccupancyGridMap2D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap2D.cpp @@ -453,7 +453,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap2D_TLikelihoodOptions : public mrpt } }; -// mrpt::maps::COccupancyGridMap2D::TMapDefinition file: line:67 +// mrpt::maps::COccupancyGridMap2D::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_COccupancyGridMap2D_TMapDefinition : public mrpt::maps::COccupancyGridMap2D::TMapDefinition { using mrpt::maps::COccupancyGridMap2D::TMapDefinition::TMapDefinition; @@ -769,12 +769,12 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s cl.def("assign", (struct mrpt::maps::COccupancyGridMap2D::TCriticalPointsList & (mrpt::maps::COccupancyGridMap2D::TCriticalPointsList::*)(const struct mrpt::maps::COccupancyGridMap2D::TCriticalPointsList &)) &mrpt::maps::COccupancyGridMap2D::TCriticalPointsList::operator=, "C++: mrpt::maps::COccupancyGridMap2D::TCriticalPointsList::operator=(const struct mrpt::maps::COccupancyGridMap2D::TCriticalPointsList &) --> struct mrpt::maps::COccupancyGridMap2D::TCriticalPointsList &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::COccupancyGridMap2D::TMapDefinitionBase file: line:62 + { // mrpt::maps::COccupancyGridMap2D::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::COccupancyGridMap2D::TMapDefinition file: line:67 + { // mrpt::maps::COccupancyGridMap2D::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_COccupancyGridMap2D_TMapDefinition, mrpt::maps::COccupancyGridMap2D::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap2D::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_COccupancyGridMap2D_TMapDefinition(); } ) ); diff --git a/python/src/mrpt/maps/COccupancyGridMap3D.cpp b/python/src/mrpt/maps/COccupancyGridMap3D.cpp index 579c80a3c5..7a271082ac 100644 --- a/python/src/mrpt/maps/COccupancyGridMap3D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap3D.cpp @@ -446,7 +446,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D_TLikelihoodOptions : public mrpt } }; -// mrpt::maps::COccupancyGridMap3D::TMapDefinition file: line:67 +// mrpt::maps::COccupancyGridMap3D::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_COccupancyGridMap3D_TMapDefinition : public mrpt::maps::COccupancyGridMap3D::TMapDefinition { using mrpt::maps::COccupancyGridMap3D::TMapDefinition::TMapDefinition; @@ -598,12 +598,12 @@ void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::s cl.def("assign", (struct mrpt::maps::COccupancyGridMap3D::TRenderingOptions & (mrpt::maps::COccupancyGridMap3D::TRenderingOptions::*)(const struct mrpt::maps::COccupancyGridMap3D::TRenderingOptions &)) &mrpt::maps::COccupancyGridMap3D::TRenderingOptions::operator=, "C++: mrpt::maps::COccupancyGridMap3D::TRenderingOptions::operator=(const struct mrpt::maps::COccupancyGridMap3D::TRenderingOptions &) --> struct mrpt::maps::COccupancyGridMap3D::TRenderingOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::COccupancyGridMap3D::TMapDefinitionBase file: line:62 + { // mrpt::maps::COccupancyGridMap3D::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::COccupancyGridMap3D::TMapDefinition file: line:67 + { // mrpt::maps::COccupancyGridMap3D::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_COccupancyGridMap3D_TMapDefinition, mrpt::maps::COccupancyGridMap3D::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap3D::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_COccupancyGridMap3D_TMapDefinition(); } ) ); diff --git a/python/src/mrpt/maps/COctoMap.cpp b/python/src/mrpt/maps/COctoMap.cpp index aaf6de25a4..67a8016c31 100644 --- a/python/src/mrpt/maps/COctoMap.cpp +++ b/python/src/mrpt/maps/COctoMap.cpp @@ -533,7 +533,7 @@ struct PyCallBack_mrpt_maps_COctoMap : public mrpt::maps::COctoMap { } }; -// mrpt::maps::COctoMap::TMapDefinition file: line:67 +// mrpt::maps::COctoMap::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_COctoMap_TMapDefinition : public mrpt::maps::COctoMap::TMapDefinition { using mrpt::maps::COctoMap::TMapDefinition::TMapDefinition; @@ -1136,7 +1136,7 @@ struct PyCallBack_mrpt_maps_CSimplePointsMap : public mrpt::maps::CSimplePointsM } }; -// mrpt::maps::CSimplePointsMap::TMapDefinition file: line:67 +// mrpt::maps::CSimplePointsMap::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_CSimplePointsMap_TMapDefinition : public mrpt::maps::CSimplePointsMap::TMapDefinition { using mrpt::maps::CSimplePointsMap::TMapDefinition::TMapDefinition; @@ -1228,12 +1228,12 @@ void bind_mrpt_maps_COctoMap(std::function< pybind11::module &(std::string const cl.def("getClampingThresMaxLog", (float (mrpt::maps::COctoMap::*)() const) &mrpt::maps::COctoMap::getClampingThresMaxLog, "C++: mrpt::maps::COctoMap::getClampingThresMaxLog() const --> float"); cl.def("assign", (class mrpt::maps::COctoMap & (mrpt::maps::COctoMap::*)(const class mrpt::maps::COctoMap &)) &mrpt::maps::COctoMap::operator=, "C++: mrpt::maps::COctoMap::operator=(const class mrpt::maps::COctoMap &) --> class mrpt::maps::COctoMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); - { // mrpt::maps::COctoMap::TMapDefinitionBase file: line:62 + { // mrpt::maps::COctoMap::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::COctoMap::TMapDefinition file: line:67 + { // mrpt::maps::COctoMap::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_COctoMap_TMapDefinition, mrpt::maps::COctoMap::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::COctoMap::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_COctoMap_TMapDefinition(); } ) ); @@ -1282,12 +1282,12 @@ void bind_mrpt_maps_COctoMap(std::function< pybind11::module &(std::string const cl.def("insertPointFast", (void (mrpt::maps::CSimplePointsMap::*)(float, float, float)) &mrpt::maps::CSimplePointsMap::insertPointFast, "The virtual method for *without* calling\n mark_as_modified() \n\nC++: mrpt::maps::CSimplePointsMap::insertPointFast(float, float, float) --> void", pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z")); cl.def("getAsSimplePointsMap", (const class mrpt::maps::CSimplePointsMap * (mrpt::maps::CSimplePointsMap::*)() const) &mrpt::maps::CSimplePointsMap::getAsSimplePointsMap, "@} \n\nC++: mrpt::maps::CSimplePointsMap::getAsSimplePointsMap() const --> const class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); - { // mrpt::maps::CSimplePointsMap::TMapDefinitionBase file: line:62 + { // mrpt::maps::CSimplePointsMap::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::CSimplePointsMap::TMapDefinition file: line:67 + { // mrpt::maps::CSimplePointsMap::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CSimplePointsMap_TMapDefinition, mrpt::maps::CSimplePointsMap::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::CSimplePointsMap::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CSimplePointsMap_TMapDefinition(); } ) ); diff --git a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp index d4a317511a..53a2c31ed3 100644 --- a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp +++ b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp @@ -674,7 +674,7 @@ struct PyCallBack_mrpt_maps_CPointsMapXYZI : public mrpt::maps::CPointsMapXYZI { } }; -// mrpt::maps::CPointsMapXYZI::TMapDefinition file: line:67 +// mrpt::maps::CPointsMapXYZI::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_CPointsMapXYZI_TMapDefinition : public mrpt::maps::CPointsMapXYZI::TMapDefinition { using mrpt::maps::CPointsMapXYZI::TMapDefinition::TMapDefinition; @@ -780,12 +780,12 @@ void bind_mrpt_maps_CPointCloudFilterByDistance(std::function< pybind11::module cl.def("hasColorPoints", (bool (mrpt::maps::CPointsMapXYZI::*)() const) &mrpt::maps::CPointsMapXYZI::hasColorPoints, "Returns true if the point map has a color field for each point \n\nC++: mrpt::maps::CPointsMapXYZI::hasColorPoints() const --> bool"); cl.def("getVisualizationInto", (void (mrpt::maps::CPointsMapXYZI::*)(class mrpt::opengl::CSetOfObjects &) const) &mrpt::maps::CPointsMapXYZI::getVisualizationInto, "Override of the default 3D scene builder to account for the individual\n points' color.\n\nC++: mrpt::maps::CPointsMapXYZI::getVisualizationInto(class mrpt::opengl::CSetOfObjects &) const --> void", pybind11::arg("outObj")); - { // mrpt::maps::CPointsMapXYZI::TMapDefinitionBase file: line:62 + { // mrpt::maps::CPointsMapXYZI::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::CPointsMapXYZI::TMapDefinition file: line:67 + { // mrpt::maps::CPointsMapXYZI::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CPointsMapXYZI_TMapDefinition, mrpt::maps::CPointsMapXYZI::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::CPointsMapXYZI::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CPointsMapXYZI_TMapDefinition(); } ) ); diff --git a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp index 918619c56d..8b5a2b8185 100644 --- a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp +++ b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp @@ -398,7 +398,7 @@ struct PyCallBack_mrpt_maps_CReflectivityGridMap2D_TInsertionOptions : public mr } }; -// mrpt::maps::CReflectivityGridMap2D::TMapDefinition file: line:67 +// mrpt::maps::CReflectivityGridMap2D::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_CReflectivityGridMap2D_TMapDefinition : public mrpt::maps::CReflectivityGridMap2D::TMapDefinition { using mrpt::maps::CReflectivityGridMap2D::TMapDefinition::TMapDefinition; @@ -969,7 +969,7 @@ struct PyCallBack_mrpt_maps_CWeightedPointsMap : public mrpt::maps::CWeightedPoi } }; -// mrpt::maps::CWeightedPointsMap::TMapDefinition file: line:67 +// mrpt::maps::CWeightedPointsMap::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_CWeightedPointsMap_TMapDefinition : public mrpt::maps::CWeightedPointsMap::TMapDefinition { using mrpt::maps::CWeightedPointsMap::TMapDefinition::TMapDefinition; @@ -1055,12 +1055,12 @@ void bind_mrpt_maps_CReflectivityGridMap2D(std::function< pybind11::module &(std cl.def("assign", (struct mrpt::maps::CReflectivityGridMap2D::TInsertionOptions & (mrpt::maps::CReflectivityGridMap2D::TInsertionOptions::*)(const struct mrpt::maps::CReflectivityGridMap2D::TInsertionOptions &)) &mrpt::maps::CReflectivityGridMap2D::TInsertionOptions::operator=, "C++: mrpt::maps::CReflectivityGridMap2D::TInsertionOptions::operator=(const struct mrpt::maps::CReflectivityGridMap2D::TInsertionOptions &) --> struct mrpt::maps::CReflectivityGridMap2D::TInsertionOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::CReflectivityGridMap2D::TMapDefinitionBase file: line:62 + { // mrpt::maps::CReflectivityGridMap2D::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::CReflectivityGridMap2D::TMapDefinition file: line:67 + { // mrpt::maps::CReflectivityGridMap2D::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CReflectivityGridMap2D_TMapDefinition, mrpt::maps::CReflectivityGridMap2D::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::CReflectivityGridMap2D::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CReflectivityGridMap2D_TMapDefinition(); } ) ); @@ -1094,12 +1094,12 @@ void bind_mrpt_maps_CReflectivityGridMap2D(std::function< pybind11::module &(std cl.def("setPointWeight", (void (mrpt::maps::CWeightedPointsMap::*)(size_t, unsigned long)) &mrpt::maps::CWeightedPointsMap::setPointWeight, "Sets the point weight, which is ignored in all classes but those which\n actually store that field (Note: No checks are done for out-of-bounds\n index). \n\n getPointWeight\n\nC++: mrpt::maps::CWeightedPointsMap::setPointWeight(size_t, unsigned long) --> void", pybind11::arg("index"), pybind11::arg("w")); cl.def("getPointWeight", (unsigned int (mrpt::maps::CWeightedPointsMap::*)(size_t) const) &mrpt::maps::CWeightedPointsMap::getPointWeight, "Gets the point weight, which is ignored in all classes (defaults to 1)\n but in those which actually store that field (Note: No checks are done\n for out-of-bounds index). \n\n setPointWeight\n\nC++: mrpt::maps::CWeightedPointsMap::getPointWeight(size_t) const --> unsigned int", pybind11::arg("index")); - { // mrpt::maps::CWeightedPointsMap::TMapDefinitionBase file: line:62 + { // mrpt::maps::CWeightedPointsMap::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::CWeightedPointsMap::TMapDefinition file: line:67 + { // mrpt::maps::CWeightedPointsMap::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CWeightedPointsMap_TMapDefinition, mrpt::maps::CWeightedPointsMap::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::CWeightedPointsMap::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CWeightedPointsMap_TMapDefinition(); } ) ); diff --git a/python/src/mrpt/maps/CVoxelMap.cpp b/python/src/mrpt/maps/CVoxelMap.cpp index 60f6c61eaf..1ba095dce0 100644 --- a/python/src/mrpt/maps/CVoxelMap.cpp +++ b/python/src/mrpt/maps/CVoxelMap.cpp @@ -396,7 +396,7 @@ struct PyCallBack_mrpt_maps_CVoxelMap : public mrpt::maps::CVoxelMap { } }; -// mrpt::maps::CVoxelMap::TMapDefinition file: line:67 +// mrpt::maps::CVoxelMap::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_CVoxelMap_TMapDefinition : public mrpt::maps::CVoxelMap::TMapDefinition { using mrpt::maps::CVoxelMap::TMapDefinition::TMapDefinition; @@ -746,7 +746,7 @@ struct PyCallBack_mrpt_maps_CVoxelMapRGB : public mrpt::maps::CVoxelMapRGB { } }; -// mrpt::maps::CVoxelMapRGB::TMapDefinition file: line:67 +// mrpt::maps::CVoxelMapRGB::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_CVoxelMapRGB_TMapDefinition : public mrpt::maps::CVoxelMapRGB::TMapDefinition { using mrpt::maps::CVoxelMapRGB::TMapDefinition::TMapDefinition; @@ -814,12 +814,12 @@ void bind_mrpt_maps_CVoxelMap(std::function< pybind11::module &(std::string cons cl.def_static("CreateObject", (class std::shared_ptr (*)()) &mrpt::maps::CVoxelMap::CreateObject, "C++: mrpt::maps::CVoxelMap::CreateObject() --> class std::shared_ptr"); cl.def("assign", (class mrpt::maps::CVoxelMap & (mrpt::maps::CVoxelMap::*)(const class mrpt::maps::CVoxelMap &)) &mrpt::maps::CVoxelMap::operator=, "C++: mrpt::maps::CVoxelMap::operator=(const class mrpt::maps::CVoxelMap &) --> class mrpt::maps::CVoxelMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); - { // mrpt::maps::CVoxelMap::TMapDefinitionBase file: line:62 + { // mrpt::maps::CVoxelMap::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::CVoxelMap::TMapDefinition file: line:67 + { // mrpt::maps::CVoxelMap::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CVoxelMap_TMapDefinition, mrpt::maps::CVoxelMap::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::CVoxelMap::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CVoxelMap_TMapDefinition(); } ) ); @@ -866,12 +866,12 @@ void bind_mrpt_maps_CVoxelMap(std::function< pybind11::module &(std::string cons cl.def_static("CreateObject", (class std::shared_ptr (*)()) &mrpt::maps::CVoxelMapRGB::CreateObject, "C++: mrpt::maps::CVoxelMapRGB::CreateObject() --> class std::shared_ptr"); cl.def("assign", (class mrpt::maps::CVoxelMapRGB & (mrpt::maps::CVoxelMapRGB::*)(const class mrpt::maps::CVoxelMapRGB &)) &mrpt::maps::CVoxelMapRGB::operator=, "C++: mrpt::maps::CVoxelMapRGB::operator=(const class mrpt::maps::CVoxelMapRGB &) --> class mrpt::maps::CVoxelMapRGB &", pybind11::return_value_policy::automatic, pybind11::arg("")); - { // mrpt::maps::CVoxelMapRGB::TMapDefinitionBase file: line:62 + { // mrpt::maps::CVoxelMapRGB::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::CVoxelMapRGB::TMapDefinition file: line:67 + { // mrpt::maps::CVoxelMapRGB::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CVoxelMapRGB_TMapDefinition, mrpt::maps::CVoxelMapRGB::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::CVoxelMapRGB::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CVoxelMapRGB_TMapDefinition(); } ) ); diff --git a/python/src/mrpt/maps/CWirelessPowerGridMap2D.cpp b/python/src/mrpt/maps/CWirelessPowerGridMap2D.cpp index 05c473597e..0f9d9bbd61 100644 --- a/python/src/mrpt/maps/CWirelessPowerGridMap2D.cpp +++ b/python/src/mrpt/maps/CWirelessPowerGridMap2D.cpp @@ -495,7 +495,7 @@ struct PyCallBack_mrpt_maps_CWirelessPowerGridMap2D_TInsertionOptions : public m } }; -// mrpt::maps::CWirelessPowerGridMap2D::TMapDefinition file: line:67 +// mrpt::maps::CWirelessPowerGridMap2D::TMapDefinition file: line:85 struct PyCallBack_mrpt_maps_CWirelessPowerGridMap2D_TMapDefinition : public mrpt::maps::CWirelessPowerGridMap2D::TMapDefinition { using mrpt::maps::CWirelessPowerGridMap2D::TMapDefinition::TMapDefinition; @@ -572,12 +572,12 @@ void bind_mrpt_maps_CWirelessPowerGridMap2D(std::function< pybind11::module &(st cl.def("assign", (struct mrpt::maps::CWirelessPowerGridMap2D::TInsertionOptions & (mrpt::maps::CWirelessPowerGridMap2D::TInsertionOptions::*)(const struct mrpt::maps::CWirelessPowerGridMap2D::TInsertionOptions &)) &mrpt::maps::CWirelessPowerGridMap2D::TInsertionOptions::operator=, "C++: mrpt::maps::CWirelessPowerGridMap2D::TInsertionOptions::operator=(const struct mrpt::maps::CWirelessPowerGridMap2D::TInsertionOptions &) --> struct mrpt::maps::CWirelessPowerGridMap2D::TInsertionOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::CWirelessPowerGridMap2D::TMapDefinitionBase file: line:62 + { // mrpt::maps::CWirelessPowerGridMap2D::TMapDefinitionBase file: line:80 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); } - { // mrpt::maps::CWirelessPowerGridMap2D::TMapDefinition file: line:67 + { // mrpt::maps::CWirelessPowerGridMap2D::TMapDefinition file: line:85 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_CWirelessPowerGridMap2D_TMapDefinition, mrpt::maps::CWirelessPowerGridMap2D::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::CWirelessPowerGridMap2D::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CWirelessPowerGridMap2D_TMapDefinition(); } ) ); diff --git a/python/src/mrpt/nav/planners/TMoveTree.cpp b/python/src/mrpt/nav/planners/TMoveTree.cpp index 4afdf81341..1370ac6c59 100644 --- a/python/src/mrpt/nav/planners/TMoveTree.cpp +++ b/python/src/mrpt/nav/planners/TMoveTree.cpp @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include @@ -42,7 +41,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/mrpt/nav/reactive/CReactiveNavigationSystem3D.cpp b/python/src/mrpt/nav/reactive/CReactiveNavigationSystem3D.cpp index 3a2ea1fa42..b5633004de 100644 --- a/python/src/mrpt/nav/reactive/CReactiveNavigationSystem3D.cpp +++ b/python/src/mrpt/nav/reactive/CReactiveNavigationSystem3D.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include #include @@ -51,7 +50,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/mrpt/poses/CPose3DPDFSOG.cpp b/python/src/mrpt/poses/CPose3DPDFSOG.cpp index 168b9e1d1f..1a3582710c 100644 --- a/python/src/mrpt/poses/CPose3DPDFSOG.cpp +++ b/python/src/mrpt/poses/CPose3DPDFSOG.cpp @@ -682,7 +682,7 @@ void bind_mrpt_poses_CPose3DPDFSOG(std::function< pybind11::module &(std::string cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::poses::CPose3DQuatPDF::*)() const) &mrpt::poses::CPose3DQuatPDF::GetRuntimeClass, "C++: mrpt::poses::CPose3DQuatPDF::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::poses::CPose3DQuatPDF::GetRuntimeClassIdStatic, "C++: mrpt::poses::CPose3DQuatPDF::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); cl.def("copyFrom", (void (mrpt::poses::CPose3DQuatPDF::*)(const class mrpt::poses::CPose3DQuatPDF &)) &mrpt::poses::CPose3DQuatPDF::copyFrom, "Copy operator, translating if necesary (for example, between particles\n and gaussian representations)\n \n\n createFrom2D\n\nC++: mrpt::poses::CPose3DQuatPDF::copyFrom(const class mrpt::poses::CPose3DQuatPDF &) --> void", pybind11::arg("o")); - cl.def_static("createFrom2D", (class mrpt::poses::CPose3DQuatPDF * (*)(const class mrpt::poses::CPosePDF &)) &mrpt::poses::CPose3DQuatPDF::createFrom2D, "This is a static transformation method from 2D poses to 3D PDFs,\n preserving the representation type (particles->particles,\n Gaussians->Gaussians,etc)\n It returns a new object of any of the derived classes of\n CPose3DQuatPDF. This object must be deleted by the user when not required\n anymore.\n \n\n copyFrom\n\nC++: mrpt::poses::CPose3DQuatPDF::createFrom2D(const class mrpt::poses::CPosePDF &) --> class mrpt::poses::CPose3DQuatPDF *", pybind11::return_value_policy::automatic, pybind11::arg("o")); + cl.def_static("createFrom2D", (class std::shared_ptr (*)(const class mrpt::poses::CPosePDF &)) &mrpt::poses::CPose3DQuatPDF::createFrom2D, "This is a static transformation method from 2D poses to 3D PDFs,\n preserving the representation type (particles->particles,\n Gaussians->Gaussians,etc)\n It returns a new object of any of the derived classes of\n CPose3DQuatPDF. This object must be deleted by the user when not required\n anymore.\n \n\n copyFrom\n\nC++: mrpt::poses::CPose3DQuatPDF::createFrom2D(const class mrpt::poses::CPosePDF &) --> class std::shared_ptr", pybind11::arg("o")); cl.def("inverse", (void (mrpt::poses::CPose3DQuatPDF::*)(class mrpt::poses::CPose3DQuatPDF &) const) &mrpt::poses::CPose3DQuatPDF::inverse, "Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF \n\nC++: mrpt::poses::CPose3DQuatPDF::inverse(class mrpt::poses::CPose3DQuatPDF &) const --> void", pybind11::arg("o")); cl.def("changeCoordinatesReference", (void (mrpt::poses::CPose3DQuatPDF::*)(const class mrpt::poses::CPose3D &)) &mrpt::poses::CPose3DQuatPDF::changeCoordinatesReference, "C++: mrpt::poses::CPose3DQuatPDF::changeCoordinatesReference(const class mrpt::poses::CPose3D &) --> void", pybind11::arg("newReferenceBase")); cl.def_static("jacobiansPoseComposition", [](const class mrpt::poses::CPose3DQuat & a0, const class mrpt::poses::CPose3DQuat & a1, class mrpt::math::CMatrixFixed & a2, class mrpt::math::CMatrixFixed & a3) -> void { return mrpt::poses::CPose3DQuatPDF::jacobiansPoseComposition(a0, a1, a2, a3); }, "", pybind11::arg("x"), pybind11::arg("u"), pybind11::arg("df_dx"), pybind11::arg("df_du")); From cb7fd9c9d156afdd4ed343043ccc8f8fa1c8b822 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 6 Nov 2023 11:00:29 +0100 Subject: [PATCH 22/36] TSetOfMetricMapInitializers throws for unknown types --- doc/source/doxygen-docs/changelog.md | 5 +- .../include/mrpt/config/CConfigFileBase.h | 16 +++++++ .../config/src/CConfigFileMemory_unittest.cpp | 8 ++-- .../src/maps/CMultiMetricMap_unittest.cpp | 47 ++++++++++++++++++- libs/obs/src/TMetricMapInitializer.cpp | 14 ++++++ 5 files changed, 82 insertions(+), 8 deletions(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 66566c450e..b89fb33636 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -2,6 +2,8 @@ # Version 2.11.3: UNRELEASED - Changes in libraries: + - \ref mrpt_core_grp + - Add the `[[nodiscard]]` attribute to all functions returning a value in `` - \ref mrpt_maps_grp - mrpt::maps::COccupancyGridMap3D::insertObservation() now also handles mrpt::obs::CObservationPointCloud - New virtual interface mrpt::maps::NearestNeighborsCapable, implemented in: @@ -11,8 +13,7 @@ - mrpt::maps::COccupancyGridMap2D - New virtual method mrpt::maps::CMetricMap::boundingBox() - mrpt::maps::TMetricMapInitializer now returns `shared_ptr`s instead of plain pointers. - - \ref mrpt_core_grp - - Add the `[[nodiscard]]` attribute to all functions returning a value in `` + - mrpt::maps::TSetOfMetricMapInitializers::loadFromConfigFile() now throws if it finds a `*_count` entry with an unknown map class name. # Version 2.11.2: Released Oct 25th, 2023 - Changes in libraries: diff --git a/libs/config/include/mrpt/config/CConfigFileBase.h b/libs/config/include/mrpt/config/CConfigFileBase.h index e047c47ddd..5f33db741e 100644 --- a/libs/config/include/mrpt/config/CConfigFileBase.h +++ b/libs/config/include/mrpt/config/CConfigFileBase.h @@ -74,10 +74,26 @@ class CConfigFileBase /** Returns a list with all the section names. */ virtual void getAllSections(std::vector& sections) const = 0; + /** Returns, by value, a list with all the section names. */ + std::vector sections() const + { + std::vector ret; + getAllSections(ret); + return ret; + } + /** Returs a list with all the keys into a section */ virtual void getAllKeys( const std::string& section, std::vector& keys) const = 0; + /** Returs, by value, a list with all the keys into a section */ + std::vector keys(const std::string& section) const + { + std::vector keys; + getAllKeys(section, keys); + return keys; + } + /** Checks if a given section exists (name is case insensitive) * \sa keyExists() */ bool sectionExists(const std::string& section_name) const; diff --git a/libs/config/src/CConfigFileMemory_unittest.cpp b/libs/config/src/CConfigFileMemory_unittest.cpp index 493ba833cd..84720ef602 100644 --- a/libs/config/src/CConfigFileMemory_unittest.cpp +++ b/libs/config/src/CConfigFileMemory_unittest.cpp @@ -25,11 +25,11 @@ TEST(CConfigFileMemory, readwrite) TEST(CConfigFileMemory, Sections) { - std::vector sections; mrpt::config::CConfigFileMemory second; second.write("one", "name", "val"); second.write("two", "names", "value"); - second.getAllSections(sections); + + const std::vector sections = second.sections(); EXPECT_EQ(2U, sections.size()); if (sections.size() == 2) { // avoid potential crash if fails @@ -40,11 +40,11 @@ TEST(CConfigFileMemory, Sections) TEST(CConfigFileMemory, Names) { - std::vector names; mrpt::config::CConfigFileMemory third; third.write("sec", "name", "val"); third.write("sec", "names", "value"); - third.getAllKeys("sec", names); + + const std::vector names = third.keys("sec"); EXPECT_EQ(2U, names.size()); if (names.size() == 2) { // avoid potential crash if fails diff --git a/libs/maps/src/maps/CMultiMetricMap_unittest.cpp b/libs/maps/src/maps/CMultiMetricMap_unittest.cpp index 43a841aa14..ccf55abad7 100644 --- a/libs/maps/src/maps/CMultiMetricMap_unittest.cpp +++ b/libs/maps/src/maps/CMultiMetricMap_unittest.cpp @@ -8,6 +8,7 @@ +------------------------------------------------------------------------+ */ #include +#include #include #include #include @@ -21,7 +22,9 @@ TEST(CMultiMetricMapTests, isEmpty) } } -static mrpt::maps::CMultiMetricMap initializer1() +namespace +{ +mrpt::maps::CMultiMetricMap initializer1() { mrpt::maps::TSetOfMetricMapInitializers map_inits; { @@ -42,13 +45,14 @@ static mrpt::maps::CMultiMetricMap initializer1() } } -static mrpt::maps::CMultiMetricMap initializer2() +mrpt::maps::CMultiMetricMap initializer2() { mrpt::maps::CMultiMetricMap m; m.maps.push_back(mrpt::maps::COccupancyGridMap2D::Create()); m.maps.push_back(mrpt::maps::CSimplePointsMap::Create()); return m; } +} // namespace TEST(CMultiMetricMapTests, initializers) { @@ -102,3 +106,42 @@ TEST(CMultiMetricMapTests, moveOp) EXPECT_EQ(m2.mapByClass()->size(), 1U); } + +TEST(CMultiMetricMapTests, unknownMapType) +{ + { + const mrpt::config::CConfigFileMemory cfg(R""""( +[map] +// Creation of maps: +occupancyGrid_count=1 + +[map_occupancyGrid_00_creationOpts] +min_x=-10 +max_x= 10 +min_y=-10 +max_y= 10 +)""""); + + mrpt::maps::TSetOfMetricMapInitializers map_inits; + EXPECT_NO_THROW(map_inits.loadFromConfigFile(cfg, "map")); + EXPECT_EQ(map_inits.size(), 1UL); + } + + { + const mrpt::config::CConfigFileMemory cfg(R""""( +[map] +// Creation of maps: +occupancyGrid_count=1 +myUnregisteredMap_count=1 + +[map_occupancyGrid_00_creationOpts] +min_x=-10 +max_x= 10 +min_y=-10 +max_y= 10 +)""""); + + mrpt::maps::TSetOfMetricMapInitializers map_inits; + EXPECT_ANY_THROW(map_inits.loadFromConfigFile(cfg, "map")); + } +} diff --git a/libs/obs/src/TMetricMapInitializer.cpp b/libs/obs/src/TMetricMapInitializer.cpp index b0a662664c..5e4e581470 100644 --- a/libs/obs/src/TMetricMapInitializer.cpp +++ b/libs/obs/src/TMetricMapInitializer.cpp @@ -109,6 +109,20 @@ void TSetOfMetricMapInitializers::loadFromConfigFile( } // end for each map kind + // Check for unknown map types and throw an error: + for (const auto& s : ini.keys(sectionName)) + { + auto p = s.find("_count"); + if (p == std::string::npos) continue; + const auto className = s.substr(0, p); + if (allMapKinds.count(className) != 0) continue; // ok, it exists + THROW_EXCEPTION_FMT( + "Error: found INI section '%s' while parsing " + "TSetOfMetricMapInitializers, but there is no such registered " + "CMetricMap class '%s'", + s.c_str(), className.c_str()); + } + MRPT_END } From d3130fc69a325589cfcd2c8563cf84c85098675e Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 6 Nov 2023 13:10:05 +0100 Subject: [PATCH 23/36] new function confidenceIntervalsFromHistogram() --- doc/source/doxygen-docs/changelog.md | 3 ++ libs/math/include/mrpt/math/distributions.h | 39 +++++++++++++++++++-- 2 files changed, 40 insertions(+), 2 deletions(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index b89fb33636..10873745bb 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -14,6 +14,9 @@ - New virtual method mrpt::maps::CMetricMap::boundingBox() - mrpt::maps::TMetricMapInitializer now returns `shared_ptr`s instead of plain pointers. - mrpt::maps::TSetOfMetricMapInitializers::loadFromConfigFile() now throws if it finds a `*_count` entry with an unknown map class name. + - \ref mrpt_math_grp + - New template mrpt::math::confidenceIntervalsFromHistogram() + # Version 2.11.2: Released Oct 25th, 2023 - Changes in libraries: diff --git a/libs/math/include/mrpt/math/distributions.h b/libs/math/include/mrpt/math/distributions.h index c585ff9509..ada78d234f 100644 --- a/libs/math/include/mrpt/math/distributions.h +++ b/libs/math/include/mrpt/math/distributions.h @@ -228,10 +228,10 @@ void confidenceIntervals( const auto x_max = data.maxCoeff(); const auto binWidth = (x_max - x_min) / histogramNumBins; - const std::vector H = + const std::vector hitsNormalized = mrpt::math::histogram(data, x_min, x_max, histogramNumBins); std::vector Hc; - cumsum(H, Hc); // CDF + cumsum(hitsNormalized, Hc); // CDF Hc *= 1.0 / mrpt::math::maximum(Hc); auto it_low = std::lower_bound(Hc.begin(), Hc.end(), confidenceInterval); @@ -247,6 +247,41 @@ void confidenceIntervals( MRPT_END } +/** Return the mean and the 10%-90% confidence points (or with any other + * confidence value) of a set of samples from their histogram. + * The container can be any MRPT container (CArray, matrices, vectors). + * \param confidenceInterval A number in the range (0,1) such as the confidence + * interval will be [100*confidenceInterval, 100*(1-confidenceInterval)]. + */ +template +void confidenceIntervalsFromHistogram( + const CONTAINER& histogramCoords, const CONTAINER& histogramNormalizedHits, + T& out_lower_conf_interval, T& out_upper_conf_interval, + const double confidenceInterval = 0.1) +{ + MRPT_START + ASSERT_(confidenceInterval > 0 && confidenceInterval < 1); + + const auto x_min = *histogramCoords.begin(); + const auto x_max = *histogramCoords.rbegin(); + const auto binWidth = (x_max - x_min) / histogramCoords.size(); + + std::vector Hc; + cumsum(histogramNormalizedHits, Hc); // CDF + Hc *= 1.0 / mrpt::math::maximum(Hc); + + auto it_low = std::lower_bound(Hc.begin(), Hc.end(), confidenceInterval); + ASSERT_(it_low != Hc.end()); + auto it_high = + std::upper_bound(Hc.begin(), Hc.end(), 1 - confidenceInterval); + ASSERT_(it_high != Hc.end()); + const size_t idx_low = std::distance(Hc.begin(), it_low); + const size_t idx_high = std::distance(Hc.begin(), it_high); + out_lower_conf_interval = x_min + idx_low * binWidth; + out_upper_conf_interval = x_min + idx_high * binWidth; + + MRPT_END +} /** @} */ } // namespace mrpt::math From 54664995294991b7cbeda130d0173b9e3872e9de Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Tue, 7 Nov 2023 00:47:05 +0100 Subject: [PATCH 24/36] Add TUM format save option --- libs/poses/include/mrpt/poses/CPose3DQuat.h | 8 +-- .../mrpt/poses/CPoseInterpolatorBase.h | 17 +++++ libs/poses/src/CPoseInterpolatorBase.hpp | 69 +++++++++++++++++++ 3 files changed, 90 insertions(+), 4 deletions(-) diff --git a/libs/poses/include/mrpt/poses/CPose3DQuat.h b/libs/poses/include/mrpt/poses/CPose3DQuat.h index bba1e61479..ff241f0b2d 100644 --- a/libs/poses/include/mrpt/poses/CPose3DQuat.h +++ b/libs/poses/include/mrpt/poses/CPose3DQuat.h @@ -273,10 +273,10 @@ class CPose3DQuat : public CPose, case 0: return m_coords[0]; case 1: return m_coords[1]; case 2: return m_coords[2]; - case 3: return m_quat[0]; - case 4: return m_quat[1]; - case 5: return m_quat[2]; - case 6: return m_quat[3]; + case 3: return m_quat[0]; // w + case 4: return m_quat[1]; // x + case 5: return m_quat[2]; // y + case 6: return m_quat[3]; // z default: throw std::runtime_error( "CPose3DQuat::operator[]: Index of bounds."); diff --git a/libs/poses/include/mrpt/poses/CPoseInterpolatorBase.h b/libs/poses/include/mrpt/poses/CPoseInterpolatorBase.h index f8d5f713d9..d3d3ec7ce6 100644 --- a/libs/poses/include/mrpt/poses/CPoseInterpolatorBase.h +++ b/libs/poses/include/mrpt/poses/CPoseInterpolatorBase.h @@ -168,6 +168,16 @@ class CPoseInterpolatorBase */ bool saveToTextFile(const std::string& s) const; + /** Saves the points in the interpolator to a text file in the "TUM" dataset + * format: each row contains these elements separated by spaces: + * - Timestamp: As a "double time_t" (see mrpt::system::timestampTotime_t). + * - x y z: The 3D position in meters. + * - q_x q_y q_z q_w: Quaternion + * \sa loadFromTextFile, saveTextFile_TUM + * \return true on success, false on any error. + */ + bool saveToTextFile_TUM(const std::string& s) const; + /** Saves the points in the interpolator to a text file, with the same * format that saveToTextFile, but interpolating the path with the given * period in seconds. @@ -183,6 +193,13 @@ class CPoseInterpolatorBase */ bool loadFromTextFile(const std::string& s); + /** Loads from a text file, in the "TUM" dataset + * format. + * \return true on success, false on any error. + * \exception std::exception On invalid file format + */ + bool loadFromTextFile_TUM(const std::string& s); + /** Computes the bounding box in all Euclidean coordinates of the whole * path. \exception std::exception On empty path */ void getBoundingBox(point_t& minCorner, point_t& maxCorner) const; diff --git a/libs/poses/src/CPoseInterpolatorBase.hpp b/libs/poses/src/CPoseInterpolatorBase.hpp index c084bbf18f..d8b1d21dbf 100644 --- a/libs/poses/src/CPoseInterpolatorBase.hpp +++ b/libs/poses/src/CPoseInterpolatorBase.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include @@ -256,6 +257,37 @@ bool CPoseInterpolatorBase::saveToTextFile(const std::string& s) const } } +template +bool CPoseInterpolatorBase::saveToTextFile_TUM(const std::string& s) const +{ + try + { + std::ofstream f; + f.open(s); + if (!f.is_open()) return false; + std::string str; + for (auto i = m_path.begin(); i != m_path.end(); ++i) + { + const double t = mrpt::system::timestampTotime_t(i->first); + const auto p = + mrpt::poses::CPose3DQuat(mrpt::poses::CPose3D(i->second)); + + // TUM format: timestamp x y z q_x q_y q_z q_w + str = mrpt::format("%.06f", t); + constexpr std::array idxs = {0, 1, 2, 4, 5, 6, 3}; + for (unsigned int k = 0; k < 7; k++) + str += mrpt::format(" %.06f", p[idxs[k]]); + str += std::string("\n"); + f << str; + } + return true; + } + catch (...) + { + return false; + } +} + template bool CPoseInterpolatorBase::saveInterpolatedToTextFile( const std::string& s, const mrpt::Clock::duration& period) const @@ -328,6 +360,43 @@ bool CPoseInterpolatorBase::loadFromTextFile(const std::string& s) MRPT_END } +template +bool CPoseInterpolatorBase::loadFromTextFile_TUM(const std::string& s) +{ + MRPT_START + + clear(); + mrpt::math::CMatrixD M; + + try + { + M.loadFromTextFile(s); + } + catch (std::exception&) + { + return false; // error loading file + } + + // Check valid format: + if (M.rows() == 0) return false; + ASSERT_(M.cols() == 3 + 4 + 1); + + // load into the path: + const size_t N = M.rows(); + constexpr std::array idxs = {0, 1, 2, 4, 5, 6, 3}; + mrpt::poses::CPose3DQuat p; + for (size_t i = 0; i < N; i++) + { + for (unsigned int k = 0; k < 7; k++) + p[idxs[k]] = M(i, k + 1); + insert( + mrpt::system::time_tToTimestamp(M(i, 0)), + pose_t(mrpt::poses::CPose3D(p).asTPose())); + } + return true; + MRPT_END +} + template void CPoseInterpolatorBase::getBoundingBox( point_t& Min, point_t& Max) const From 0bdf3c62a6d537255a8fb01b86de4752ba541239 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Tue, 7 Nov 2023 10:37:03 +0100 Subject: [PATCH 25/36] workaround MSVC build error --- libs/opengl/src/CSkyBox.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libs/opengl/src/CSkyBox.cpp b/libs/opengl/src/CSkyBox.cpp index e45baf912a..6ccde31934 100644 --- a/libs/opengl/src/CSkyBox.cpp +++ b/libs/opengl/src/CSkyBox.cpp @@ -27,7 +27,7 @@ IMPLEMENTS_SERIALIZABLE(CSkyBox, CRenderizable, mrpt::opengl) void CSkyBox::renderUpdateBuffers() const { // vertices: - constexpr thread_local float skyboxVertices[] = { + constexpr static float skyboxVertices[] = { // Positions -1.0f, 1.0f, -1.0f, // -1.0f, -1.0f, -1.0f, // From 02dd54047f92e0b6eb4221c5b80eb4b2d3d29869 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Tue, 7 Nov 2023 17:15:12 +0100 Subject: [PATCH 26/36] pymrpt: fix armhf build error --- python/patch-011.diff | 276 ++++++++++++++++++ python/src/mrpt/maps/CColouredOctoMap.cpp | 4 +- python/src/mrpt/maps/COccupancyGridMap2D.cpp | 8 +- python/src/mrpt/maps/COccupancyGridMap3D.cpp | 8 +- python/src/mrpt/maps/COctoMap.cpp | 4 +- .../mrpt/maps/CPointCloudFilterByDistance.cpp | 4 +- python/src/mrpt/maps/CPointsMap.cpp | 4 +- .../src/mrpt/maps/CReflectivityGridMap2D.cpp | 4 +- python/src/mrpt/maps/CVoxelMap.cpp | 8 +- .../mrpt/maps/CVoxelMapOccupancyBase_1.cpp | 8 +- .../mrpt/maps/CVoxelMapOccupancyBase_2.cpp | 8 +- .../src/mrpt/maps/NearestNeighborsCapable.cpp | 4 +- 12 files changed, 308 insertions(+), 32 deletions(-) create mode 100644 python/patch-011.diff diff --git a/python/patch-011.diff b/python/patch-011.diff new file mode 100644 index 0000000000..4a84290295 --- /dev/null +++ b/python/patch-011.diff @@ -0,0 +1,276 @@ +diff --git a/python/src/mrpt/maps/CColouredOctoMap.cpp b/python/src/mrpt/maps/CColouredOctoMap.cpp +index 69357914c..e50023c9f 100644 +--- a/python/src/mrpt/maps/CColouredOctoMap.cpp ++++ b/python/src/mrpt/maps/CColouredOctoMap.cpp +@@ -1012,7 +1012,7 @@ struct PyCallBack_mrpt_maps_CColouredPointsMap : public mrpt::maps::CColouredPoi + } + return CPointsMap::nn_index_count(); + } +- bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +@@ -1025,7 +1025,7 @@ struct PyCallBack_mrpt_maps_CColouredPointsMap : public mrpt::maps::CColouredPoi + } + return CPointsMap::nn_single_search(a0, a1, a2, a3); + } +- bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +diff --git a/python/src/mrpt/maps/COccupancyGridMap2D.cpp b/python/src/mrpt/maps/COccupancyGridMap2D.cpp +index e6e7c2594..6a8f7845c 100644 +--- a/python/src/mrpt/maps/COccupancyGridMap2D.cpp ++++ b/python/src/mrpt/maps/COccupancyGridMap2D.cpp +@@ -309,7 +309,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap2D : public mrpt::maps::COccupancyG + } + return COccupancyGridMap2D::nn_index_count(); + } +- bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +@@ -322,7 +322,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap2D : public mrpt::maps::COccupancyG + } + return COccupancyGridMap2D::nn_single_search(a0, a1, a2, a3); + } +- bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +@@ -637,8 +637,8 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s + cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap2D::asString() const --> std::string"); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::nn_has_indices_or_ids, " @{ \n\nC++: mrpt::maps::COccupancyGridMap2D::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::nn_index_count, "C++: mrpt::maps::COccupancyGridMap2D::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("assign", (class mrpt::maps::COccupancyGridMap2D & (mrpt::maps::COccupancyGridMap2D::*)(const class mrpt::maps::COccupancyGridMap2D &)) &mrpt::maps::COccupancyGridMap2D::operator=, "C++: mrpt::maps::COccupancyGridMap2D::operator=(const class mrpt::maps::COccupancyGridMap2D &) --> class mrpt::maps::COccupancyGridMap2D &", pybind11::return_value_policy::automatic, pybind11::arg("")); + + { // mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly file:mrpt/maps/COccupancyGridMap2D.h line:215 +diff --git a/python/src/mrpt/maps/COccupancyGridMap3D.cpp b/python/src/mrpt/maps/COccupancyGridMap3D.cpp +index 7a271082a..0da94c502 100644 +--- a/python/src/mrpt/maps/COccupancyGridMap3D.cpp ++++ b/python/src/mrpt/maps/COccupancyGridMap3D.cpp +@@ -302,7 +302,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D : public mrpt::maps::COccupancyG + } + return COccupancyGridMap3D::nn_index_count(); + } +- bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +@@ -315,7 +315,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D : public mrpt::maps::COccupancyG + } + return COccupancyGridMap3D::nn_single_search(a0, a1, a2, a3); + } +- bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +@@ -541,8 +541,8 @@ void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::s + cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap3D::asString() const --> std::string"); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::nn_has_indices_or_ids, " @{ \n\nC++: mrpt::maps::COccupancyGridMap3D::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::nn_index_count, "C++: mrpt::maps::COccupancyGridMap3D::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("assign", (class mrpt::maps::COccupancyGridMap3D & (mrpt::maps::COccupancyGridMap3D::*)(const class mrpt::maps::COccupancyGridMap3D &)) &mrpt::maps::COccupancyGridMap3D::operator=, "C++: mrpt::maps::COccupancyGridMap3D::operator=(const class mrpt::maps::COccupancyGridMap3D &) --> class mrpt::maps::COccupancyGridMap3D &", pybind11::return_value_policy::automatic, pybind11::arg("")); + + { // mrpt::maps::COccupancyGridMap3D::TInsertionOptions file:mrpt/maps/COccupancyGridMap3D.h line:221 +diff --git a/python/src/mrpt/maps/COctoMap.cpp b/python/src/mrpt/maps/COctoMap.cpp +index 67a8016c3..97447db55 100644 +--- a/python/src/mrpt/maps/COctoMap.cpp ++++ b/python/src/mrpt/maps/COctoMap.cpp +@@ -1017,7 +1017,7 @@ struct PyCallBack_mrpt_maps_CSimplePointsMap : public mrpt::maps::CSimplePointsM + } + return CPointsMap::nn_index_count(); + } +- bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +@@ -1030,7 +1030,7 @@ struct PyCallBack_mrpt_maps_CSimplePointsMap : public mrpt::maps::CSimplePointsM + } + return CPointsMap::nn_single_search(a0, a1, a2, a3); + } +- bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +diff --git a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp +index 53a2c31ed..283cffcd2 100644 +--- a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp ++++ b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp +@@ -581,7 +581,7 @@ struct PyCallBack_mrpt_maps_CPointsMapXYZI : public mrpt::maps::CPointsMapXYZI { + } + return CPointsMap::nn_index_count(); + } +- bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +@@ -594,7 +594,7 @@ struct PyCallBack_mrpt_maps_CPointsMapXYZI : public mrpt::maps::CPointsMapXYZI { + } + return CPointsMap::nn_single_search(a0, a1, a2, a3); + } +- bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +diff --git a/python/src/mrpt/maps/CPointsMap.cpp b/python/src/mrpt/maps/CPointsMap.cpp +index 1b940df9a..a29c05e9a 100644 +--- a/python/src/mrpt/maps/CPointsMap.cpp ++++ b/python/src/mrpt/maps/CPointsMap.cpp +@@ -255,8 +255,8 @@ void bind_mrpt_maps_CPointsMap(std::function< pybind11::module &(std::string con + cl.def("asString", (std::string (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::CPointsMap::asString() const --> std::string"); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::nn_has_indices_or_ids, " @{ \n\nC++: mrpt::maps::CPointsMap::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::nn_index_count, "C++: mrpt::maps::CPointsMap::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + + { // mrpt::maps::CPointsMap::TInsertionOptions file:mrpt/maps/CPointsMap.h line:228 + auto & enclosing_class = cl; +diff --git a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp +index 8b5a2b818..02ac97b7b 100644 +--- a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp ++++ b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp +@@ -850,7 +850,7 @@ struct PyCallBack_mrpt_maps_CWeightedPointsMap : public mrpt::maps::CWeightedPoi + } + return CPointsMap::nn_index_count(); + } +- bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +@@ -863,7 +863,7 @@ struct PyCallBack_mrpt_maps_CWeightedPointsMap : public mrpt::maps::CWeightedPoi + } + return CPointsMap::nn_single_search(a0, a1, a2, a3); + } +- bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +diff --git a/python/src/mrpt/maps/CVoxelMap.cpp b/python/src/mrpt/maps/CVoxelMap.cpp +index 1ba095dce..f042679cc 100644 +--- a/python/src/mrpt/maps/CVoxelMap.cpp ++++ b/python/src/mrpt/maps/CVoxelMap.cpp +@@ -238,7 +238,7 @@ struct PyCallBack_mrpt_maps_CVoxelMap : public mrpt::maps::CVoxelMap { + } + return CVoxelMapOccupancyBase::nn_index_count(); + } +- bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +@@ -251,7 +251,7 @@ struct PyCallBack_mrpt_maps_CVoxelMap : public mrpt::maps::CVoxelMap { + } + return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2, a3); + } +- bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +@@ -588,7 +588,7 @@ struct PyCallBack_mrpt_maps_CVoxelMapRGB : public mrpt::maps::CVoxelMapRGB { + } + return CVoxelMapOccupancyBase::nn_index_count(); + } +- bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +@@ -601,7 +601,7 @@ struct PyCallBack_mrpt_maps_CVoxelMapRGB : public mrpt::maps::CVoxelMapRGB { + } + return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2, a3); + } +- bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { ++ bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); + if (overload) { +diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp +index 56901034a..dade459d9 100644 +--- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp ++++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp +@@ -69,8 +69,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s + cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_index_count, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); + cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); + cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); +@@ -103,8 +103,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s + cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids, "Returns true if the rest of `nn_*` methods will populate the output\n indices values with 0-based contiguous **indices**.\n Returns false if indices are actually sparse **ID numbers** without any\n expectation of they be contiguous or start near zero.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_has_indices_or_ids() returns `true`, this must return the number\n of \"points\" (or whatever entity) the indices correspond to. Otherwise,\n the return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); + } + } +diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp +index 48fc11853..e5b22eac1 100644 +--- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp ++++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp +@@ -69,8 +69,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s + cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_index_count, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); + cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); + cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); +@@ -103,8 +103,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s + cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids, "Returns true if the rest of `nn_*` methods will populate the output\n indices values with 0-based contiguous **indices**.\n Returns false if indices are actually sparse **ID numbers** without any\n expectation of they be contiguous or start near zero.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_has_indices_or_ids() returns `true`, this must return the number\n of \"points\" (or whatever entity) the indices correspond to. Otherwise,\n the return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); + } + } +diff --git a/python/src/mrpt/maps/NearestNeighborsCapable.cpp b/python/src/mrpt/maps/NearestNeighborsCapable.cpp +index a22f929c1..b4d515565 100644 +--- a/python/src/mrpt/maps/NearestNeighborsCapable.cpp ++++ b/python/src/mrpt/maps/NearestNeighborsCapable.cpp +@@ -28,8 +28,8 @@ void bind_mrpt_maps_NearestNeighborsCapable(std::function< pybind11::module &(st + pybind11::class_> cl(M("mrpt::maps"), "NearestNeighborsCapable", "Virtual interface for maps having the capability of searching the closest\n neighbor(s) of a given query 2D or 3D point.\n\n Note this is more generic than mrpt::math::KDTreeCapable since it does not\n assume the use of KD-trees, and it is also non templatized, so users can use\n dynamic casting to interact with maps in a generic way.\n\n \n New in MRPT 2.11.3\n \n\n\n "); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids, "Returns true if the rest of `nn_*` methods will populate the output\n indices values with 0-based contiguous **indices**.\n Returns false if indices are actually sparse **ID numbers** without any\n expectation of they be contiguous or start near zero.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_has_indices_or_ids() returns `true`, this must return the number\n of \"points\" (or whatever entity) the indices correspond to. Otherwise,\n the return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); + } + } diff --git a/python/src/mrpt/maps/CColouredOctoMap.cpp b/python/src/mrpt/maps/CColouredOctoMap.cpp index 69357914cb..e50023c9f7 100644 --- a/python/src/mrpt/maps/CColouredOctoMap.cpp +++ b/python/src/mrpt/maps/CColouredOctoMap.cpp @@ -1012,7 +1012,7 @@ struct PyCallBack_mrpt_maps_CColouredPointsMap : public mrpt::maps::CColouredPoi } return CPointsMap::nn_index_count(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { @@ -1025,7 +1025,7 @@ struct PyCallBack_mrpt_maps_CColouredPointsMap : public mrpt::maps::CColouredPoi } return CPointsMap::nn_single_search(a0, a1, a2, a3); } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { diff --git a/python/src/mrpt/maps/COccupancyGridMap2D.cpp b/python/src/mrpt/maps/COccupancyGridMap2D.cpp index e6e7c25944..6a8f7845c1 100644 --- a/python/src/mrpt/maps/COccupancyGridMap2D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap2D.cpp @@ -309,7 +309,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap2D : public mrpt::maps::COccupancyG } return COccupancyGridMap2D::nn_index_count(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { @@ -322,7 +322,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap2D : public mrpt::maps::COccupancyG } return COccupancyGridMap2D::nn_single_search(a0, a1, a2, a3); } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { @@ -637,8 +637,8 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap2D::asString() const --> std::string"); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::nn_has_indices_or_ids, " @{ \n\nC++: mrpt::maps::COccupancyGridMap2D::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::nn_index_count, "C++: mrpt::maps::COccupancyGridMap2D::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); cl.def("assign", (class mrpt::maps::COccupancyGridMap2D & (mrpt::maps::COccupancyGridMap2D::*)(const class mrpt::maps::COccupancyGridMap2D &)) &mrpt::maps::COccupancyGridMap2D::operator=, "C++: mrpt::maps::COccupancyGridMap2D::operator=(const class mrpt::maps::COccupancyGridMap2D &) --> class mrpt::maps::COccupancyGridMap2D &", pybind11::return_value_policy::automatic, pybind11::arg("")); { // mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly file:mrpt/maps/COccupancyGridMap2D.h line:215 diff --git a/python/src/mrpt/maps/COccupancyGridMap3D.cpp b/python/src/mrpt/maps/COccupancyGridMap3D.cpp index 7a271082ac..0da94c5025 100644 --- a/python/src/mrpt/maps/COccupancyGridMap3D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap3D.cpp @@ -302,7 +302,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D : public mrpt::maps::COccupancyG } return COccupancyGridMap3D::nn_index_count(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { @@ -315,7 +315,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D : public mrpt::maps::COccupancyG } return COccupancyGridMap3D::nn_single_search(a0, a1, a2, a3); } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { @@ -541,8 +541,8 @@ void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::s cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap3D::asString() const --> std::string"); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::nn_has_indices_or_ids, " @{ \n\nC++: mrpt::maps::COccupancyGridMap3D::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::nn_index_count, "C++: mrpt::maps::COccupancyGridMap3D::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); cl.def("assign", (class mrpt::maps::COccupancyGridMap3D & (mrpt::maps::COccupancyGridMap3D::*)(const class mrpt::maps::COccupancyGridMap3D &)) &mrpt::maps::COccupancyGridMap3D::operator=, "C++: mrpt::maps::COccupancyGridMap3D::operator=(const class mrpt::maps::COccupancyGridMap3D &) --> class mrpt::maps::COccupancyGridMap3D &", pybind11::return_value_policy::automatic, pybind11::arg("")); { // mrpt::maps::COccupancyGridMap3D::TInsertionOptions file:mrpt/maps/COccupancyGridMap3D.h line:221 diff --git a/python/src/mrpt/maps/COctoMap.cpp b/python/src/mrpt/maps/COctoMap.cpp index 67a8016c31..97447db55f 100644 --- a/python/src/mrpt/maps/COctoMap.cpp +++ b/python/src/mrpt/maps/COctoMap.cpp @@ -1017,7 +1017,7 @@ struct PyCallBack_mrpt_maps_CSimplePointsMap : public mrpt::maps::CSimplePointsM } return CPointsMap::nn_index_count(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { @@ -1030,7 +1030,7 @@ struct PyCallBack_mrpt_maps_CSimplePointsMap : public mrpt::maps::CSimplePointsM } return CPointsMap::nn_single_search(a0, a1, a2, a3); } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { diff --git a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp index 53a2c31ed3..283cffcd2e 100644 --- a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp +++ b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp @@ -581,7 +581,7 @@ struct PyCallBack_mrpt_maps_CPointsMapXYZI : public mrpt::maps::CPointsMapXYZI { } return CPointsMap::nn_index_count(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { @@ -594,7 +594,7 @@ struct PyCallBack_mrpt_maps_CPointsMapXYZI : public mrpt::maps::CPointsMapXYZI { } return CPointsMap::nn_single_search(a0, a1, a2, a3); } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { diff --git a/python/src/mrpt/maps/CPointsMap.cpp b/python/src/mrpt/maps/CPointsMap.cpp index 1b940df9a7..a29c05e9a8 100644 --- a/python/src/mrpt/maps/CPointsMap.cpp +++ b/python/src/mrpt/maps/CPointsMap.cpp @@ -255,8 +255,8 @@ void bind_mrpt_maps_CPointsMap(std::function< pybind11::module &(std::string con cl.def("asString", (std::string (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::CPointsMap::asString() const --> std::string"); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::nn_has_indices_or_ids, " @{ \n\nC++: mrpt::maps::CPointsMap::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::nn_index_count, "C++: mrpt::maps::CPointsMap::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); { // mrpt::maps::CPointsMap::TInsertionOptions file:mrpt/maps/CPointsMap.h line:228 auto & enclosing_class = cl; diff --git a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp index 8b5a2b8185..02ac97b7bd 100644 --- a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp +++ b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp @@ -850,7 +850,7 @@ struct PyCallBack_mrpt_maps_CWeightedPointsMap : public mrpt::maps::CWeightedPoi } return CPointsMap::nn_index_count(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { @@ -863,7 +863,7 @@ struct PyCallBack_mrpt_maps_CWeightedPointsMap : public mrpt::maps::CWeightedPoi } return CPointsMap::nn_single_search(a0, a1, a2, a3); } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { diff --git a/python/src/mrpt/maps/CVoxelMap.cpp b/python/src/mrpt/maps/CVoxelMap.cpp index 1ba095dce0..f042679cc9 100644 --- a/python/src/mrpt/maps/CVoxelMap.cpp +++ b/python/src/mrpt/maps/CVoxelMap.cpp @@ -238,7 +238,7 @@ struct PyCallBack_mrpt_maps_CVoxelMap : public mrpt::maps::CVoxelMap { } return CVoxelMapOccupancyBase::nn_index_count(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { @@ -251,7 +251,7 @@ struct PyCallBack_mrpt_maps_CVoxelMap : public mrpt::maps::CVoxelMap { } return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2, a3); } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { @@ -588,7 +588,7 @@ struct PyCallBack_mrpt_maps_CVoxelMapRGB : public mrpt::maps::CVoxelMapRGB { } return CVoxelMapOccupancyBase::nn_index_count(); } - bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint3D_ & a0, struct mrpt::math::TPoint3D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { @@ -601,7 +601,7 @@ struct PyCallBack_mrpt_maps_CVoxelMapRGB : public mrpt::maps::CVoxelMapRGB { } return CVoxelMapOccupancyBase::nn_single_search(a0, a1, a2, a3); } - bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, unsigned long & a3) const override { + bool nn_single_search(const struct mrpt::math::TPoint2D_ & a0, struct mrpt::math::TPoint2D_ & a1, float & a2, uint64_t & a3) const override { pybind11::gil_scoped_acquire gil; pybind11::function overload = pybind11::get_overload(static_cast(this), "nn_single_search"); if (overload) { diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp index 56901034ab..dade459d9c 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp @@ -69,8 +69,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_index_count, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); @@ -103,8 +103,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids, "Returns true if the rest of `nn_*` methods will populate the output\n indices values with 0-based contiguous **indices**.\n Returns false if indices are actually sparse **ID numbers** without any\n expectation of they be contiguous or start near zero.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_has_indices_or_ids() returns `true`, this must return the number\n of \"points\" (or whatever entity) the indices correspond to. Otherwise,\n the return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp index 48fc11853e..e5b22eac12 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp @@ -69,8 +69,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_index_count, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); @@ -103,8 +103,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids, "Returns true if the rest of `nn_*` methods will populate the output\n indices values with 0-based contiguous **indices**.\n Returns false if indices are actually sparse **ID numbers** without any\n expectation of they be contiguous or start near zero.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_has_indices_or_ids() returns `true`, this must return the number\n of \"points\" (or whatever entity) the indices correspond to. Otherwise,\n the return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/maps/NearestNeighborsCapable.cpp b/python/src/mrpt/maps/NearestNeighborsCapable.cpp index a22f929c1a..b4d5155652 100644 --- a/python/src/mrpt/maps/NearestNeighborsCapable.cpp +++ b/python/src/mrpt/maps/NearestNeighborsCapable.cpp @@ -28,8 +28,8 @@ void bind_mrpt_maps_NearestNeighborsCapable(std::function< pybind11::module &(st pybind11::class_> cl(M("mrpt::maps"), "NearestNeighborsCapable", "Virtual interface for maps having the capability of searching the closest\n neighbor(s) of a given query 2D or 3D point.\n\n Note this is more generic than mrpt::math::KDTreeCapable since it does not\n assume the use of KD-trees, and it is also non templatized, so users can use\n dynamic casting to interact with maps in a generic way.\n\n \n New in MRPT 2.11.3\n \n\n\n "); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids, "Returns true if the rest of `nn_*` methods will populate the output\n indices values with 0-based contiguous **indices**.\n Returns false if indices are actually sparse **ID numbers** without any\n expectation of they be contiguous or start near zero.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_has_indices_or_ids() returns `true`, this must return the number\n of \"points\" (or whatever entity) the indices correspond to. Otherwise,\n the return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } From c4e09e25ba225feb64b17396b351de1d23394738 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 8 Nov 2023 12:53:30 +0100 Subject: [PATCH 27/36] more armhf fixes --- python/patch-012.diff | 112 ++++++++++++++++++ python/src/mrpt/maps/COccupancyGridMap2D.cpp | 4 +- python/src/mrpt/maps/COccupancyGridMap3D.cpp | 4 +- python/src/mrpt/maps/CPointsMap.cpp | 4 +- .../mrpt/maps/CVoxelMapOccupancyBase_1.cpp | 8 +- .../mrpt/maps/CVoxelMapOccupancyBase_2.cpp | 8 +- .../src/mrpt/maps/NearestNeighborsCapable.cpp | 4 +- 7 files changed, 128 insertions(+), 16 deletions(-) create mode 100644 python/patch-012.diff diff --git a/python/patch-012.diff b/python/patch-012.diff new file mode 100644 index 0000000000..072448cbf7 --- /dev/null +++ b/python/patch-012.diff @@ -0,0 +1,112 @@ +diff --git a/python/src/mrpt/maps/COccupancyGridMap2D.cpp b/python/src/mrpt/maps/COccupancyGridMap2D.cpp +index 6a8f7845c..59755fef0 100644 +--- a/python/src/mrpt/maps/COccupancyGridMap2D.cpp ++++ b/python/src/mrpt/maps/COccupancyGridMap2D.cpp +@@ -637,8 +637,8 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s + cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap2D::asString() const --> std::string"); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::nn_has_indices_or_ids, " @{ \n\nC++: mrpt::maps::COccupancyGridMap2D::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::nn_index_count, "C++: mrpt::maps::COccupancyGridMap2D::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("assign", (class mrpt::maps::COccupancyGridMap2D & (mrpt::maps::COccupancyGridMap2D::*)(const class mrpt::maps::COccupancyGridMap2D &)) &mrpt::maps::COccupancyGridMap2D::operator=, "C++: mrpt::maps::COccupancyGridMap2D::operator=(const class mrpt::maps::COccupancyGridMap2D &) --> class mrpt::maps::COccupancyGridMap2D &", pybind11::return_value_policy::automatic, pybind11::arg("")); + + { // mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly file:mrpt/maps/COccupancyGridMap2D.h line:215 +diff --git a/python/src/mrpt/maps/COccupancyGridMap3D.cpp b/python/src/mrpt/maps/COccupancyGridMap3D.cpp +index 0da94c502..166f1cc88 100644 +--- a/python/src/mrpt/maps/COccupancyGridMap3D.cpp ++++ b/python/src/mrpt/maps/COccupancyGridMap3D.cpp +@@ -541,8 +541,8 @@ void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::s + cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap3D::asString() const --> std::string"); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::nn_has_indices_or_ids, " @{ \n\nC++: mrpt::maps::COccupancyGridMap3D::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::nn_index_count, "C++: mrpt::maps::COccupancyGridMap3D::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("assign", (class mrpt::maps::COccupancyGridMap3D & (mrpt::maps::COccupancyGridMap3D::*)(const class mrpt::maps::COccupancyGridMap3D &)) &mrpt::maps::COccupancyGridMap3D::operator=, "C++: mrpt::maps::COccupancyGridMap3D::operator=(const class mrpt::maps::COccupancyGridMap3D &) --> class mrpt::maps::COccupancyGridMap3D &", pybind11::return_value_policy::automatic, pybind11::arg("")); + + { // mrpt::maps::COccupancyGridMap3D::TInsertionOptions file:mrpt/maps/COccupancyGridMap3D.h line:221 +diff --git a/python/src/mrpt/maps/CPointsMap.cpp b/python/src/mrpt/maps/CPointsMap.cpp +index a29c05e9a..3d2ece918 100644 +--- a/python/src/mrpt/maps/CPointsMap.cpp ++++ b/python/src/mrpt/maps/CPointsMap.cpp +@@ -255,8 +255,8 @@ void bind_mrpt_maps_CPointsMap(std::function< pybind11::module &(std::string con + cl.def("asString", (std::string (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::CPointsMap::asString() const --> std::string"); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::nn_has_indices_or_ids, " @{ \n\nC++: mrpt::maps::CPointsMap::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::nn_index_count, "C++: mrpt::maps::CPointsMap::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + + { // mrpt::maps::CPointsMap::TInsertionOptions file:mrpt/maps/CPointsMap.h line:228 + auto & enclosing_class = cl; +diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp +index dade459d9..67a288932 100644 +--- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp ++++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp +@@ -69,8 +69,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s + cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_index_count, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); + cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); + cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); +@@ -103,8 +103,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s + cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids, "Returns true if the rest of `nn_*` methods will populate the output\n indices values with 0-based contiguous **indices**.\n Returns false if indices are actually sparse **ID numbers** without any\n expectation of they be contiguous or start near zero.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_has_indices_or_ids() returns `true`, this must return the number\n of \"points\" (or whatever entity) the indices correspond to. Otherwise,\n the return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); + } + } +diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp +index e5b22eac1..49b74e052 100644 +--- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp ++++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp +@@ -69,8 +69,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s + cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_index_count, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); + cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); + cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); +@@ -103,8 +103,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s + cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids, "Returns true if the rest of `nn_*` methods will populate the output\n indices values with 0-based contiguous **indices**.\n Returns false if indices are actually sparse **ID numbers** without any\n expectation of they be contiguous or start near zero.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_has_indices_or_ids() returns `true`, this must return the number\n of \"points\" (or whatever entity) the indices correspond to. Otherwise,\n the return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); + } + } +diff --git a/python/src/mrpt/maps/NearestNeighborsCapable.cpp b/python/src/mrpt/maps/NearestNeighborsCapable.cpp +index b4d515565..ab164dbca 100644 +--- a/python/src/mrpt/maps/NearestNeighborsCapable.cpp ++++ b/python/src/mrpt/maps/NearestNeighborsCapable.cpp +@@ -28,8 +28,8 @@ void bind_mrpt_maps_NearestNeighborsCapable(std::function< pybind11::module &(st + pybind11::class_> cl(M("mrpt::maps"), "NearestNeighborsCapable", "Virtual interface for maps having the capability of searching the closest\n neighbor(s) of a given query 2D or 3D point.\n\n Note this is more generic than mrpt::math::KDTreeCapable since it does not\n assume the use of KD-trees, and it is also non templatized, so users can use\n dynamic casting to interact with maps in a generic way.\n\n \n New in MRPT 2.11.3\n \n\n\n "); + cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids, "Returns true if the rest of `nn_*` methods will populate the output\n indices values with 0-based contiguous **indices**.\n Returns false if indices are actually sparse **ID numbers** without any\n expectation of they be contiguous or start near zero.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids() const --> bool"); + cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_has_indices_or_ids() returns `true`, this must return the number\n of \"points\" (or whatever entity) the indices correspond to. Otherwise,\n the return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); +- cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); +- cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); ++ cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); + } + } diff --git a/python/src/mrpt/maps/COccupancyGridMap2D.cpp b/python/src/mrpt/maps/COccupancyGridMap2D.cpp index 6a8f7845c1..59755fef0f 100644 --- a/python/src/mrpt/maps/COccupancyGridMap2D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap2D.cpp @@ -637,8 +637,8 @@ void bind_mrpt_maps_COccupancyGridMap2D(std::function< pybind11::module &(std::s cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap2D::asString() const --> std::string"); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::nn_has_indices_or_ids, " @{ \n\nC++: mrpt::maps::COccupancyGridMap2D::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::COccupancyGridMap2D::*)() const) &mrpt::maps::COccupancyGridMap2D::nn_index_count, "C++: mrpt::maps::COccupancyGridMap2D::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap2D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::COccupancyGridMap2D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap2D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); cl.def("assign", (class mrpt::maps::COccupancyGridMap2D & (mrpt::maps::COccupancyGridMap2D::*)(const class mrpt::maps::COccupancyGridMap2D &)) &mrpt::maps::COccupancyGridMap2D::operator=, "C++: mrpt::maps::COccupancyGridMap2D::operator=(const class mrpt::maps::COccupancyGridMap2D &) --> class mrpt::maps::COccupancyGridMap2D &", pybind11::return_value_policy::automatic, pybind11::arg("")); { // mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly file:mrpt/maps/COccupancyGridMap2D.h line:215 diff --git a/python/src/mrpt/maps/COccupancyGridMap3D.cpp b/python/src/mrpt/maps/COccupancyGridMap3D.cpp index 0da94c5025..166f1cc882 100644 --- a/python/src/mrpt/maps/COccupancyGridMap3D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap3D.cpp @@ -541,8 +541,8 @@ void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::s cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap3D::asString() const --> std::string"); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::nn_has_indices_or_ids, " @{ \n\nC++: mrpt::maps::COccupancyGridMap3D::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::nn_index_count, "C++: mrpt::maps::COccupancyGridMap3D::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::COccupancyGridMap3D::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::COccupancyGridMap3D::nn_single_search, "C++: mrpt::maps::COccupancyGridMap3D::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); cl.def("assign", (class mrpt::maps::COccupancyGridMap3D & (mrpt::maps::COccupancyGridMap3D::*)(const class mrpt::maps::COccupancyGridMap3D &)) &mrpt::maps::COccupancyGridMap3D::operator=, "C++: mrpt::maps::COccupancyGridMap3D::operator=(const class mrpt::maps::COccupancyGridMap3D &) --> class mrpt::maps::COccupancyGridMap3D &", pybind11::return_value_policy::automatic, pybind11::arg("")); { // mrpt::maps::COccupancyGridMap3D::TInsertionOptions file:mrpt/maps/COccupancyGridMap3D.h line:221 diff --git a/python/src/mrpt/maps/CPointsMap.cpp b/python/src/mrpt/maps/CPointsMap.cpp index a29c05e9a8..3d2ece918e 100644 --- a/python/src/mrpt/maps/CPointsMap.cpp +++ b/python/src/mrpt/maps/CPointsMap.cpp @@ -255,8 +255,8 @@ void bind_mrpt_maps_CPointsMap(std::function< pybind11::module &(std::string con cl.def("asString", (std::string (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::CPointsMap::asString() const --> std::string"); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::nn_has_indices_or_ids, " @{ \n\nC++: mrpt::maps::CPointsMap::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::CPointsMap::*)() const) &mrpt::maps::CPointsMap::nn_index_count, "C++: mrpt::maps::CPointsMap::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::CPointsMap::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::CPointsMap::nn_single_search, "C++: mrpt::maps::CPointsMap::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); { // mrpt::maps::CPointsMap::TInsertionOptions file:mrpt/maps/CPointsMap.h line:228 auto & enclosing_class = cl; diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp index dade459d9c..67a288932b 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp @@ -69,8 +69,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_index_count, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); @@ -103,8 +103,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids, "Returns true if the rest of `nn_*` methods will populate the output\n indices values with 0-based contiguous **indices**.\n Returns false if indices are actually sparse **ID numbers** without any\n expectation of they be contiguous or start near zero.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_has_indices_or_ids() returns `true`, this must return the number\n of \"points\" (or whatever entity) the indices correspond to. Otherwise,\n the return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp index e5b22eac12..49b74e0529 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp @@ -69,8 +69,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::nn_index_count, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::CVoxelMapOccupancyBase::nn_single_search, "C++: mrpt::maps::CVoxelMapOccupancyBase::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrID")); cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); @@ -103,8 +103,8 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids, "Returns true if the rest of `nn_*` methods will populate the output\n indices values with 0-based contiguous **indices**.\n Returns false if indices are actually sparse **ID numbers** without any\n expectation of they be contiguous or start near zero.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_has_indices_or_ids() returns `true`, this must return the number\n of \"points\" (or whatever entity) the indices correspond to. Otherwise,\n the return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/maps/NearestNeighborsCapable.cpp b/python/src/mrpt/maps/NearestNeighborsCapable.cpp index b4d5155652..ab164dbca6 100644 --- a/python/src/mrpt/maps/NearestNeighborsCapable.cpp +++ b/python/src/mrpt/maps/NearestNeighborsCapable.cpp @@ -28,8 +28,8 @@ void bind_mrpt_maps_NearestNeighborsCapable(std::function< pybind11::module &(st pybind11::class_> cl(M("mrpt::maps"), "NearestNeighborsCapable", "Virtual interface for maps having the capability of searching the closest\n neighbor(s) of a given query 2D or 3D point.\n\n Note this is more generic than mrpt::math::KDTreeCapable since it does not\n assume the use of KD-trees, and it is also non templatized, so users can use\n dynamic casting to interact with maps in a generic way.\n\n \n New in MRPT 2.11.3\n \n\n\n "); cl.def("nn_has_indices_or_ids", (bool (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids, "Returns true if the rest of `nn_*` methods will populate the output\n indices values with 0-based contiguous **indices**.\n Returns false if indices are actually sparse **ID numbers** without any\n expectation of they be contiguous or start near zero.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_has_indices_or_ids() const --> bool"); cl.def("nn_index_count", (size_t (mrpt::maps::NearestNeighborsCapable::*)() const) &mrpt::maps::NearestNeighborsCapable::nn_index_count, "If nn_has_indices_or_ids() returns `true`, this must return the number\n of \"points\" (or whatever entity) the indices correspond to. Otherwise,\n the return value should be ignored.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_index_count() const --> size_t"); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); - cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, unsigned long &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "Search for the closest 3D point to a given one.\n\n \n The query input point.\n \n\n The found closest point.\n \n\n The square Euclidean distance between the query\n and the returned point.\n \n\n The index or ID of the result point in the\n map.\n\n \n True if successful, false if no point was found.\n\nC++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint3D_ &, struct mrpt::math::TPoint3D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); + cl.def("nn_single_search", (bool (mrpt::maps::NearestNeighborsCapable::*)(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const) &mrpt::maps::NearestNeighborsCapable::nn_single_search, "C++: mrpt::maps::NearestNeighborsCapable::nn_single_search(const struct mrpt::math::TPoint2D_ &, struct mrpt::math::TPoint2D_ &, float &, uint64_t &) const --> bool", pybind11::arg("query"), pybind11::arg("result"), pybind11::arg("out_dist_sqr"), pybind11::arg("resultIndexOrIDOrID")); cl.def("assign", (class mrpt::maps::NearestNeighborsCapable & (mrpt::maps::NearestNeighborsCapable::*)(const class mrpt::maps::NearestNeighborsCapable &)) &mrpt::maps::NearestNeighborsCapable::operator=, "C++: mrpt::maps::NearestNeighborsCapable::operator=(const class mrpt::maps::NearestNeighborsCapable &) --> class mrpt::maps::NearestNeighborsCapable &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } From aebf74be971c11b8d8c1cc1edcbda82de0d0e984 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Thu, 9 Nov 2023 00:13:46 +0100 Subject: [PATCH 28/36] FIX: MCP_SAVE() with class enums --- doc/source/doxygen-docs/changelog.md | 2 ++ .../containers/include/mrpt/containers/yaml.h | 22 ++++++++++++++----- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 10873745bb..4735991392 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -16,6 +16,8 @@ - mrpt::maps::TSetOfMetricMapInitializers::loadFromConfigFile() now throws if it finds a `*_count` entry with an unknown map class name. - \ref mrpt_math_grp - New template mrpt::math::confidenceIntervalsFromHistogram() +- BUG FIXES: + - Fix compilation errors if using the `MCP_SAVE()` macro with class enum types. # Version 2.11.2: Released Oct 25th, 2023 diff --git a/libs/containers/include/mrpt/containers/yaml.h b/libs/containers/include/mrpt/containers/yaml.h index 6ee0b8c371..c376ae8b67 100644 --- a/libs/containers/include/mrpt/containers/yaml.h +++ b/libs/containers/include/mrpt/containers/yaml.h @@ -912,6 +912,22 @@ std::ostream& operator<<(std::ostream& o, const yaml& p); MCP_LOAD_OPT(Yaml__, Var__); \ Var__ = mrpt::DEG2RAD(Var__) +namespace internal +{ +// We need to implement this as a template for the "if constexpr()" false branch +// not to be evaluated for enums, which would lead to build errors. +template +void impl_mcp_save(YAML_T& y, const T& var, const char* varName) +{ + using enum_t = std::remove_cv_t; + + if constexpr (std::is_enum_v) + y[varName] = mrpt::typemeta::TEnumType::value2name(var); + else + y[varName] = var; +} +} // namespace internal + /** Macro to store a variable into a mrpt::containers::yaml (initials MCP) * dictionary, using as "key" the name of the variable. * @@ -931,11 +947,7 @@ std::ostream& operator<<(std::ostream& o, const yaml& p); * values. Note that this requires enums to implement mrpt::typemeta::TEnumType. */ #define MCP_SAVE(Yaml__, Var__) \ - if constexpr (std::is_enum_v) \ - Yaml__[#Var__] = mrpt::typemeta::TEnumType< \ - std::remove_cv_t>::value2name(Var__); \ - else \ - Yaml__[#Var__] = Var__; + mrpt::containers::internal::impl_mcp_save(Yaml__, Var__, #Var__); #define MCP_SAVE_DEG(Yaml__, Var__) Yaml__[#Var__] = mrpt::RAD2DEG(Var__); From 8f76bb3df664c558624d9fb72599879006dac729 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sat, 11 Nov 2023 01:09:40 +0100 Subject: [PATCH 29/36] BUGFIX: Wrong pose in insertObs --- doc/source/doxygen-docs/changelog.md | 2 +- libs/maps/src/maps/CPointsMap.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 4735991392..8ad8b088c6 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -18,7 +18,7 @@ - New template mrpt::math::confidenceIntervalsFromHistogram() - BUG FIXES: - Fix compilation errors if using the `MCP_SAVE()` macro with class enum types. - + - Fix wrong cloud pose in CPointsMap::insertObservation() when inserting an mrpt::obs::CObservationPointCloud. # Version 2.11.2: Released Oct 25th, 2023 - Changes in libraries: diff --git a/libs/maps/src/maps/CPointsMap.cpp b/libs/maps/src/maps/CPointsMap.cpp index e650ec6a2e..95a9d67219 100644 --- a/libs/maps/src/maps/CPointsMap.cpp +++ b/libs/maps/src/maps/CPointsMap.cpp @@ -1930,7 +1930,8 @@ bool CPointsMap::internal_insertObservation( { // Don't fuse: Simply add insertionOptions.addToExistingPointsMap = true; - this->insertAnotherMap(o.pointcloud.get(), o.sensorPose); + this->insertAnotherMap( + o.pointcloud.get(), robotPose3D + o.sensorPose); } return true; } From 921a7db623abf34b8227fb3ed3760a9a0da3fc9f Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sat, 11 Nov 2023 23:02:40 +0100 Subject: [PATCH 30/36] Mod in NN API --- libs/maps/include/mrpt/maps/COccupancyGridMap2D.h | 6 ++++-- libs/maps/include/mrpt/maps/COccupancyGridMap3D.h | 6 ++++-- libs/maps/include/mrpt/maps/CPointsMap.h | 6 ++++-- libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h | 10 ++++++---- .../maps/include/mrpt/maps/NearestNeighborsCapable.h | 7 +++++-- libs/maps/src/maps/COccupancyGridMap2D_common.cpp | 12 ++++++------ libs/maps/src/maps/COccupancyGridMap3D.cpp | 6 ++++-- libs/maps/src/maps/CPointsMap.cpp | 7 ++++--- 8 files changed, 37 insertions(+), 23 deletions(-) diff --git a/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h b/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h index b143f5e02a..fcbb7c8e80 100644 --- a/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h +++ b/libs/maps/include/mrpt/maps/COccupancyGridMap2D.h @@ -1195,12 +1195,14 @@ class COccupancyGridMap2D const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const override; + std::vector& resultIndicesOrIDs, + size_t maxPoints) const override; void nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const override; + std::vector& resultIndicesOrIDs, + size_t maxPoints) const override; /** @} */ private: diff --git a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h index 0d17aaa4e9..b399898f1a 100644 --- a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h +++ b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h @@ -422,12 +422,14 @@ class COccupancyGridMap3D const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const override; + std::vector& resultIndicesOrIDs, + size_t maxPoints) const override; void nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const override; + std::vector& resultIndicesOrIDs, + size_t maxPoints) const override; /** @} */ private: diff --git a/libs/maps/include/mrpt/maps/CPointsMap.h b/libs/maps/include/mrpt/maps/CPointsMap.h index 62d0f17aca..30728ef6b6 100644 --- a/libs/maps/include/mrpt/maps/CPointsMap.h +++ b/libs/maps/include/mrpt/maps/CPointsMap.h @@ -1150,12 +1150,14 @@ class CPointsMap : public CMetricMap, const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const override; + std::vector& resultIndicesOrIDs, + size_t maxPoints) const override; void nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const override; + std::vector& resultIndicesOrIDs, + size_t maxPoints) const override; /** @} */ protected: diff --git a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h index 3d7201a421..fc00eb4139 100644 --- a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h +++ b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h @@ -336,21 +336,23 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase, const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const override + std::vector& resultIndicesOrIDs, + size_t maxPoints) const override { getOccupiedVoxels()->nn_radius_search( query, search_radius_sqr, results, out_dists_sqr, - resultIndicesOrIDs); + resultIndicesOrIDs, maxPoints); } void nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const override + std::vector& resultIndicesOrIDs, + size_t maxPoints) const override { getOccupiedVoxels()->nn_radius_search( query, search_radius_sqr, results, out_dists_sqr, - resultIndicesOrIDs); + resultIndicesOrIDs, maxPoints); } /** @} */ diff --git a/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h b/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h index 2023d04eb1..73e08913c1 100644 --- a/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h +++ b/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h @@ -95,19 +95,22 @@ class NearestNeighborsCapable * \param[out] out_dists_sqr The square Euclidean distances between the * query and the returned point. * \param[out] resultIndicesOrIDs The indices or IDs of the result points. + * \param[in] maxPoints If !=0, the maximum number of neigbors to return. */ virtual void nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const = 0; + std::vector& resultIndicesOrIDs, + size_t maxPoints = 0) const = 0; /// \overload for 2D points virtual void nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const = 0; + std::vector& resultIndicesOrIDs, + size_t maxPoints = 0) const = 0; /** @} */ }; diff --git a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp index 6ef5ae0cc4..a6ddb6ed05 100644 --- a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp +++ b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp @@ -798,13 +798,13 @@ void COccupancyGridMap2D::nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const + std::vector& resultIndicesOrIDs, size_t maxPoints) const { // delegate to the 2D version: std::vector r; nn_radius_search( {query.x, query.y}, search_radius_sqr, r, out_dists_sqr, - resultIndicesOrIDs); + resultIndicesOrIDs, maxPoints); results.resize(r.size()); for (size_t i = 0; i < r.size(); i++) results[i] = {r[i].x, r[i].y, .0f}; @@ -905,7 +905,7 @@ void COccupancyGridMap2D::nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const + std::vector& resultIndicesOrIDs, size_t maxPoints) const { results.clear(); out_dists_sqr.clear(); @@ -937,9 +937,7 @@ void COccupancyGridMap2D::nn_radius_search( mrpt::saturate(cx1, 0, getSizeX() - 1); mrpt::saturate(cy1, 0, getSizeY() - 1); - auto lambdaAddCell = [maxSearchRadiusSqrInCells, cx_query, cy_query, - &out_dists_sqr, &results, resolutionSqr, - &resultIndicesOrIDs, this](int cx, int cy) { + auto lambdaAddCell = [&](int cx, int cy) { int distSqr = mrpt::square(cx - cx_query) + mrpt::square(cy - cy_query); if (distSqr > maxSearchRadiusSqrInCells) return; @@ -964,5 +962,7 @@ void COccupancyGridMap2D::nn_radius_search( if (int cx = cx1; m_map[cx + cy * m_size_x] < thresholdCellValue) lambdaAddCell(cx, cy); } + + if (maxPoints && results.size() >= maxPoints) break; } } diff --git a/libs/maps/src/maps/COccupancyGridMap3D.cpp b/libs/maps/src/maps/COccupancyGridMap3D.cpp index 8f32de28bc..06fab4f55f 100644 --- a/libs/maps/src/maps/COccupancyGridMap3D.cpp +++ b/libs/maps/src/maps/COccupancyGridMap3D.cpp @@ -510,7 +510,7 @@ void COccupancyGridMap3D::nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const + std::vector& resultIndicesOrIDs, size_t maxPoints) const { THROW_EXCEPTION("Cannot run a 2D search on a 3D gridmap"); } @@ -639,7 +639,7 @@ void COccupancyGridMap3D::nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const + std::vector& resultIndicesOrIDs, size_t maxPoints) const { results.clear(); out_dists_sqr.clear(); @@ -729,5 +729,7 @@ void COccupancyGridMap3D::nn_radius_search( lambdaAddCell(cx, cy, cz); } } + + if (maxPoints && results.size() >= maxPoints) break; } } diff --git a/libs/maps/src/maps/CPointsMap.cpp b/libs/maps/src/maps/CPointsMap.cpp index 95a9d67219..bb2102c256 100644 --- a/libs/maps/src/maps/CPointsMap.cpp +++ b/libs/maps/src/maps/CPointsMap.cpp @@ -2171,7 +2171,7 @@ void CPointsMap::nn_radius_search( const mrpt::math::TPoint3Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const + std::vector& resultIndicesOrIDs, size_t maxPoints) const { std::vector> indices_dist; kdTreeRadiusSearch3D( @@ -2192,11 +2192,12 @@ void CPointsMap::nn_radius_search( const mrpt::math::TPoint2Df& query, const float search_radius_sqr, std::vector& results, std::vector& out_dists_sqr, - std::vector& resultIndicesOrIDs) const + std::vector& resultIndicesOrIDs, size_t maxPoints) const { std::vector> indices_dist; kdTreeRadiusSearch2D(query.x, query.y, search_radius_sqr, indices_dist); - const size_t nResults = indices_dist.size(); + size_t nResults = indices_dist.size(); + if (maxPoints && nResults > maxPoints) nResults = maxPoints; results.resize(nResults); out_dists_sqr.resize(nResults); resultIndicesOrIDs.resize(nResults); From 2ce9fdf82183e22e97f1ac680fb99cd778cfac16 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 12 Nov 2023 22:58:21 +0100 Subject: [PATCH 31/36] more efficient NN searches (less mem allocs) --- libs/maps/src/maps/CPointsMap.cpp | 96 +++++++++++++++++++++++-------- 1 file changed, 72 insertions(+), 24 deletions(-) diff --git a/libs/maps/src/maps/CPointsMap.cpp b/libs/maps/src/maps/CPointsMap.cpp index bb2102c256..d777e15bc2 100644 --- a/libs/maps/src/maps/CPointsMap.cpp +++ b/libs/maps/src/maps/CPointsMap.cpp @@ -2173,19 +2173,43 @@ void CPointsMap::nn_radius_search( std::vector& out_dists_sqr, std::vector& resultIndicesOrIDs, size_t maxPoints) const { - std::vector> indices_dist; - kdTreeRadiusSearch3D( - query.x, query.y, query.z, search_radius_sqr, indices_dist); - const size_t nResults = indices_dist.size(); - results.resize(nResults); - out_dists_sqr.resize(nResults); - resultIndicesOrIDs.resize(nResults); - for (size_t i = 0; i < nResults; i++) + if (maxPoints) { - getPointFast( - indices_dist[i].first, results[i].x, results[i].y, results[i].z); - out_dists_sqr[i] = indices_dist[i].second; - resultIndicesOrIDs[i] = indices_dist[i].first; + std::vector idxs; + kdTreeNClosestPoint3DIdx( + query.x, query.y, query.z, maxPoints, idxs, out_dists_sqr); + results.resize(idxs.size()); + resultIndicesOrIDs.resize(idxs.size()); + for (size_t i = 0; i < idxs.size(); i++) + { + if (out_dists_sqr[i] > search_radius_sqr) // truncate list? + { + results.resize(i); + out_dists_sqr.resize(i); + resultIndicesOrIDs.resize(i); + break; + } + getPointFast(idxs[i], results[i].x, results[i].y, results[i].z); + resultIndicesOrIDs[i] = idxs[i]; + } + } + else + { + std::vector> indices_dist; + kdTreeRadiusSearch3D( + query.x, query.y, query.z, search_radius_sqr, indices_dist); + const size_t nResults = indices_dist.size(); + results.resize(nResults); + out_dists_sqr.resize(nResults); + resultIndicesOrIDs.resize(nResults); + for (size_t i = 0; i < nResults; i++) + { + getPointFast( + indices_dist[i].first, results[i].x, results[i].y, + results[i].z); + out_dists_sqr[i] = indices_dist[i].second; + resultIndicesOrIDs[i] = indices_dist[i].first; + } } } void CPointsMap::nn_radius_search( @@ -2194,18 +2218,42 @@ void CPointsMap::nn_radius_search( std::vector& out_dists_sqr, std::vector& resultIndicesOrIDs, size_t maxPoints) const { - std::vector> indices_dist; - kdTreeRadiusSearch2D(query.x, query.y, search_radius_sqr, indices_dist); - size_t nResults = indices_dist.size(); - if (maxPoints && nResults > maxPoints) nResults = maxPoints; - results.resize(nResults); - out_dists_sqr.resize(nResults); - resultIndicesOrIDs.resize(nResults); - float dummyZ = 0; - for (size_t i = 0; i < nResults; i++) + if (maxPoints) { - getPointFast(indices_dist[i].first, results[i].x, results[i].y, dummyZ); - out_dists_sqr[i] = indices_dist[i].second; - resultIndicesOrIDs[i] = indices_dist[i].first; + std::vector idxs; + kdTreeNClosestPoint2DIdx( + query.x, query.y, maxPoints, idxs, out_dists_sqr); + results.resize(idxs.size()); + resultIndicesOrIDs.resize(idxs.size()); + float dummyZ = 0; + for (size_t i = 0; i < idxs.size(); i++) + { + if (out_dists_sqr[i] > search_radius_sqr) // truncate list? + { + results.resize(i); + out_dists_sqr.resize(i); + resultIndicesOrIDs.resize(i); + break; + } + getPointFast(idxs[i], results[i].x, results[i].y, dummyZ); + resultIndicesOrIDs[i] = idxs[i]; + } + } + else + { + std::vector> indices_dist; + kdTreeRadiusSearch2D(query.x, query.y, search_radius_sqr, indices_dist); + const size_t nResults = indices_dist.size(); + results.resize(nResults); + out_dists_sqr.resize(nResults); + resultIndicesOrIDs.resize(nResults); + float dummyZ = 0; + for (size_t i = 0; i < nResults; i++) + { + getPointFast( + indices_dist[i].first, results[i].x, results[i].y, dummyZ); + out_dists_sqr[i] = indices_dist[i].second; + resultIndicesOrIDs[i] = indices_dist[i].first; + } } } From 640865e6aacce132f7829279c2a6c90b1f28bfbc Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sat, 18 Nov 2023 01:02:40 +0100 Subject: [PATCH 32/36] remove unregistered map name --- apps/RawLogViewer/CFormRawMap.cpp | 2 +- .../include/mrpt/maps/TMetricMapInitializer.h | 2 - .../benchmark-options.ini | 1 - .../_demo_map.ini | 1 - .../grid-matching/gridmatch_example.ini | 1 - share/mrpt/config_files/hmt-slam/malaga.ini | 234 ------------------ .../config_files/icp-slam/icp-gas-mapping.ini | 1 - .../icp-slam/icp-slam_demo_LM.ini | 1 - .../icp-slam/icp-slam_demo_classic.ini | 1 - .../icp-slam_demo_classic_gridmatch.ini | 1 - .../pf-localization/localization_demo.ini | 1 - .../localization_demo_obsonly_rawlog.ini | 1 - .../rbpf-slam/RO-SLAM_simulatedData_MC.ini | 1 - .../rbpf-slam/RO-SLAM_simulatedData_SOG.ini | 1 - .../rbpf-slam/example_3_gridmaps.ini | 1 - .../rbpf-slam/gas_mapping_2lasers_2enoses.ini | 1 - .../gridmapping_RBPF_ICPbased_intel.ini | 1 - .../gridmapping_RBPF_ICPbased_malaga.ini | 1 - .../gridmapping_RBPF_grid_ICPbased_malaga.ini | 1 - .../rbpf-slam/gridmapping_intel.ini | 1 - .../gridmapping_optimal_sampling.ini | 1 - tests/montecarlo_test1.ini | 3 +- 22 files changed, 2 insertions(+), 257 deletions(-) delete mode 100644 share/mrpt/config_files/hmt-slam/malaga.ini diff --git a/apps/RawLogViewer/CFormRawMap.cpp b/apps/RawLogViewer/CFormRawMap.cpp index f250fd037e..39320ac397 100644 --- a/apps/RawLogViewer/CFormRawMap.cpp +++ b/apps/RawLogViewer/CFormRawMap.cpp @@ -287,7 +287,7 @@ CFormRawMap::CFormRawMap(wxWindow* parent, wxWindowID) "MULTIMETRIC MAP CONFIGURATION\n; " "====================================================\n[map]\n; " "Creation of maps:\noccupancyGrid_count = 0\ngasGrid_count = " - "0\nlandmarksMap_count = 0\nbeaconMap_count = 0\npointsMap_count = " + "0\nbeaconMap_count = 0\npointsMap_count = " "1\nheightMap_count = 0\ncolourPointsMap_count=0\n\n; " "====================================================\n; MULTIMETRIC " "MAP: PointsMap #00\n; " diff --git a/libs/obs/include/mrpt/maps/TMetricMapInitializer.h b/libs/obs/include/mrpt/maps/TMetricMapInitializer.h index bd03f10aa6..a1abca67bb 100644 --- a/libs/obs/include/mrpt/maps/TMetricMapInitializer.h +++ b/libs/obs/include/mrpt/maps/TMetricMapInitializer.h @@ -124,8 +124,6 @@ class TSetOfMetricMapInitializers : public mrpt::config::CLoadableOptions * colourOctoMap_count= * gasGrid_count= * wifiGrid_count= - * landmarksMap_count=<0 or 1, for creating a mrpt::maps::CLandmarksMap - *map> * beaconMap_count=<0 or 1, for creating a mrpt::maps::CBeaconMap map> * pointsMap_count= * heightMap_count= diff --git a/samples/maps_gridmap_benchmark/benchmark-options.ini b/samples/maps_gridmap_benchmark/benchmark-options.ini index c579138086..d514a39cea 100644 --- a/samples/maps_gridmap_benchmark/benchmark-options.ini +++ b/samples/maps_gridmap_benchmark/benchmark-options.ini @@ -11,7 +11,6 @@ ; Creation of maps: occupancyGrid_count=1 gasGrid_count=0 -landmarksMap_count=0 beaconMap_count=0 pointsMap_count=0 diff --git a/samples/slam_range_only_localization_rej_sampling_example/_demo_map.ini b/samples/slam_range_only_localization_rej_sampling_example/_demo_map.ini index 7549f67b7b..711cc40feb 100644 --- a/samples/slam_range_only_localization_rej_sampling_example/_demo_map.ini +++ b/samples/slam_range_only_localization_rej_sampling_example/_demo_map.ini @@ -15,7 +15,6 @@ ; Creation of maps: occupancyGrid_count=0 gasGrid_count=0 -landmarksMap_count=1 pointsMap_count=0 beaconMap_count=0 diff --git a/share/mrpt/config_files/grid-matching/gridmatch_example.ini b/share/mrpt/config_files/grid-matching/gridmatch_example.ini index c2ccd54ad2..e60bed87a7 100644 --- a/share/mrpt/config_files/grid-matching/gridmatch_example.ini +++ b/share/mrpt/config_files/grid-matching/gridmatch_example.ini @@ -64,7 +64,6 @@ LogPolarImagesOptions.num_angles = 8 [metric_maps] occupancyGrid_count=1 gasGrid_count=0 -landmarksMap_count=0 beaconMap_count=0 pointsMap_count=1 diff --git a/share/mrpt/config_files/hmt-slam/malaga.ini b/share/mrpt/config_files/hmt-slam/malaga.ini deleted file mode 100644 index a08c31143b..0000000000 --- a/share/mrpt/config_files/hmt-slam/malaga.ini +++ /dev/null @@ -1,234 +0,0 @@ -#------------------------------------------------------ -# Config file for the Hierarchical Mapping Framework -# -# ~ The MRPT Library ~ -# Jose Luis Blanco Claraco � 2005-2006 -#------------------------------------------------------ - - -#==================================================== -# -# HMT-SLAM -# -# Here come global parameters for the app. -#==================================================== -[HMT-SLAM] - -# The source file (RAW-LOG) with action/observation pairs - -rawlog_file=/Rawlogs/2006-01ENE-21-Telecom Faculty_PLS_only.rawlog -#rawlog_file=/Rawlogs/importadosCARMEN/fr_campus.rawlog -#rawlog_file=/Rawlogs/MOOS/Jericho2006/Jericho_decimated4.rawlog -#rawlog_file=/Rawlogs/importadosCARMEN/intel.rawlog -#rawlog_file=/Rawlogs/importadosCARMEN/2002-09-11-MIT_Infinite_Corridor.rawlog -#rawlog_file=/Rawlogs/2006-10OCT-26_Sancho_Floor2.2_LARGE_2laser_icpodo.rawlog -#rawlog_file=/Rawlogs/2006-01ENE-20-Corridor2.3_many_times_to_test_ICP.rawlog - -# The directory where the log files will be saved (left in blank if no log is required) -LOG_OUTPUT_DIR = LOG_HTMSLAM_MALAGA - -rawlog_offset = 0 // Whether to skip some rawlog entries -LOG_FREQUENCY = 20 // The frequency of log files generation: -LOG_SHOW3D = 1 -random_seed = 1234 // 0:Randomize, !=0:use that seed. - -# -------------------------------- -# Local SLAM method selection: -# 1: RBPF_2DLASER -# -------------------------------- -SLAM_METHOD=1 - -#SLAM_MIN_DIST_BETWEEN_OBS=1.0 // Map updates threshold (meters) -#SLAM_MIN_HEADING_BETWEEN_OBS_DEG=50 // Map updates threshold (degrees) - -SLAM_MIN_DIST_BETWEEN_OBS=1.25 // Map updates threshold (meters) -SLAM_MIN_HEADING_BETWEEN_OBS_DEG=30 // Map updates threshold (degrees) - -MIN_ODOMETRY_STD_XY = 0.05 // Minimum sigma in odometry increments (meters) -MIN_ODOMETRY_STD_PHI = 2 // Minimum sigma in odometry increments (deg) - -# Loop closure detectors: -# gridmaps -# images -TLC_DETECTORS=gridmaps - -# ==================================================== -# TLC_GRIDMATCHING -# -# Top. Loop-closure detector based on grid-matching -# ==================================================== -[TLC_GRIDMATCHING] -featsPerSquareMeter = 0.012 - -threshold_max = 0.20 // For considering candidate matches -threshold_delta = 0.09 - -ransac_prob_good_inliers = 0.9999999999 // Prob. of a good inliers (for the number of iterations). - -maxKLd_for_merge = 0.9 // Merge of close SOG modes - -min_ICP_goodness = 0.25 -max_ICP_mahadist = 20 //10 // The maximum Mahalanobis distance between the initial and final poses in the ICP not to discard the hypothesis (default=10) - -ransac_minSetSizeRatio = 0.15 // 0.20 - -ransac_mahalanobisDistanceThreshold = 6 // amRobust method only -ransac_chi2_quantile = 0.5 // amModifiedRANSAC method only - -save_feat_coors = 0 // Dump correspondences to grid_feats -debug_save_map_pairs = 1 // Save the pair of maps with the best correspondences -debug_show_corrs = 0 // Debug output of graphs - - -# ---------------------------------------------------------- -# All the params of the feature detectors/descriptors -# ---------------------------------------------------------- -featsType = 1 // 0: KLT, 1: Harris, 3: SIFT, 4: SURF - -# The feature descriptor to use: 0=detector already has descriptor, -# 1= SIFT, 2=SURF, 4=Spin images, 8=Polar images, 16=log-polar images -feature_descriptor = 8 - -patchSize = 0 // Not needed - -KLTOptions.min_distance = 6 // Pixels -KLTOptions.threshold = 0.01 // 0.10 // 0.20 - -harrisOptions.min_distance = 6 // Pixels -harrisOptions.threshold = 0.10 // 0.20 - -SURFOptions.rotation_invariant = 1 // 0=64 dims, 1=128dims - -SpinImagesOptions.hist_size_distance = 10 -SpinImagesOptions.hist_size_intensity = 10 -SpinImagesOptions.radius = 20 - -PolarImagesOptions.bins_angle = 8 -PolarImagesOptions.bins_distance = 6 -PolarImagesOptions.radius = 40 - -LogPolarImagesOptions.radius = 20 -LogPolarImagesOptions.num_angles = 8 - - - -# ==================================================== -# -# PARTICLE_FILTER -# -# Parameters of the PARTICLE FILTER within each LMH, -# invoked & implemented in CLSLAM_RBPF_2DLASER -# ==================================================== -[PARTICLE_FILTER] -#---------------------------------------------------------------------------------- -# The Particle Filter algorithm: -# 0: pfStandardProposal -# 1: pfAuxiliaryPFStandard -# 2: pfOptimalProposal *** (ICP,...) -# 3: pfAuxiliaryPFOptimal *** (Optimal SAMPLING) -# -# See: http://babel.isa.uma.es/mrpt/index.php/Particle_Filters -#---------------------------------------------------------------------------------- -PF_algorithm=3 - -adaptiveSampleSize = 0 // 0: Fixed # of particles, 1: KLD adaptive - -#---------------------------------------------------------------------------------- -# The Particle Filter Resampling method: -# 0: prMultinomial -# 1: prResidual -# 2: prStratified -# 3: prSystematic -# -# See: /docs/html/topic_resampling.html or http://mrpt.sourceforge.net/topic_resampling.html -#---------------------------------------------------------------------------------- -resamplingMethod=0 -pfAuxFilterOptimal_MaximumSearchSamples = 250 // For PF algorithm=3 - -sampleSize = 5 // Number of particles (for fixed number algorithms) -BETA = 0.50 // Resampling ESS threshold -powFactor = 0.01 // A "power factor" for updating weights - - -# ==================================================== -# GRAPH_CUT -# -# Params for Area Abstraction (AA) -# ==================================================== -[GRAPH_CUT] -partitionThreshold = 0.6 // In the range [0,1]. Lower gives larger clusters. -minDistForCorrespondence = 0.50 -useMapMatching = 1 -minimumNumberElementsEachCluster = 5 - -# ==================================================== -# -# MULTIMETRIC MAP CONFIGURATION -# -# The params for creating the metric maps for -# each LMH. -# ==================================================== -[MetricMaps] -# Creation of maps: -occupancyGrid_count = 1 -gasGrid_count = 0 -landmarksMap_count = 0 -beaconMap_count = 0 -pointsMap_count = 1 - -# Selection of map for likelihood: (fuseAll=-1,occGrid=0, points=1,landmarks=2,gasGrid=3) -likelihoodMapSelection = 0 - -# Enables (1) / Disables (0) insertion into specific maps: -enableInsertion_pointsMap = 1 -enableInsertion_landmarksMap= 1 -enableInsertion_beaconMap = 1 -enableInsertion_gridMaps = 1 -enableInsertion_gasGridMaps = 1 - -# ==================================================== -# MULTIMETRIC MAP: OccGrid #00 -# ==================================================== -# Creation Options for OccupancyGridMap 00: -[MetricMaps_occupancyGrid_00_creationOpts] -resolution=0.07 -disableSaveAs3DObject=0 - - -# Insertion Options for OccupancyGridMap 00: -[MetricMaps_occupancyGrid_00_insertOpts] -mapAltitude = 0 -useMapAltitude = 0 -maxDistanceInsertion = 35 -maxOccupancyUpdateCertainty = 0.60 -considerInvalidRangesAsFreeSpace = 1 -minLaserScanNoiseStd = 0.001 -horizontalTolerance = 0.9 // In degrees - -CFD_features_gaussian_size = 3 -CFD_features_median_size = 3 - - -# Likelihood Options for OccupancyGridMap 00: -[MetricMaps_occupancyGrid_00_likelihoodOpts] -likelihoodMethod = 4 // 0=MI, 1=Beam Model, 2=RSLC, 3=Cells Difs, 4=LF_Thrun, 5=LF_II -LF_decimation = 4 -LF_stdHit = 0.10 -LF_maxCorrsDistance = 0.50 -LF_zHit = 0.999 -LF_zRandom = 0.001 -LF_maxRange = 60 -LF_alternateAverageMethod = 0 -enableLikelihoodCache = 1 - -# ==================================================== -# MULTIMETRIC MAP: PointMap #00 -# ==================================================== -# Creation Options for Pointsmap 00: -# Creation Options for OccupancyGridMap 00: -[MetricMaps_PointsMap_00_creationOpts] -disableSaveAs3DObject=0 - -[MetricMaps_PointsMap_00_insertOpts] -minDistBetweenLaserPoints=0.05 // The minimum distance between points (in 3D): If two points are too close, one of them is not inserted into the map. -isPlanarMap=0 // If set to true, only HORIZONTAL (i.e. XY plane) measurements will be inserted in the map. Default value is false, thus 3D maps are generated diff --git a/share/mrpt/config_files/icp-slam/icp-gas-mapping.ini b/share/mrpt/config_files/icp-slam/icp-gas-mapping.ini index fe2d4b5e40..f95123ea17 100644 --- a/share/mrpt/config_files/icp-slam/icp-gas-mapping.ini +++ b/share/mrpt/config_files/icp-slam/icp-gas-mapping.ini @@ -72,7 +72,6 @@ alwaysInsertByClass = CObservationGasSensors occupancyGrid_count=1 gasGrid_count=1 ;number of gridmaps to generate wifiGrid_count=0 ;number of gridmaps to generate -landmarksMap_count=0 beaconMap_count=0 pointsMap_count=1 diff --git a/share/mrpt/config_files/icp-slam/icp-slam_demo_LM.ini b/share/mrpt/config_files/icp-slam/icp-slam_demo_LM.ini index b1d43cd7c4..6e478a2d30 100644 --- a/share/mrpt/config_files/icp-slam/icp-slam_demo_LM.ini +++ b/share/mrpt/config_files/icp-slam/icp-slam_demo_LM.ini @@ -71,7 +71,6 @@ matchAgainstTheGrid = 0 # Creation of maps: occupancyGrid_count=0 gasGrid_count=0 -landmarksMap_count=0 beaconMap_count=0 pointsMap_count=1 diff --git a/share/mrpt/config_files/icp-slam/icp-slam_demo_classic.ini b/share/mrpt/config_files/icp-slam/icp-slam_demo_classic.ini index 22149530a0..53d47851a0 100644 --- a/share/mrpt/config_files/icp-slam/icp-slam_demo_classic.ini +++ b/share/mrpt/config_files/icp-slam/icp-slam_demo_classic.ini @@ -70,7 +70,6 @@ matchAgainstTheGrid = 0 # Creation of maps: occupancyGrid_count=0 gasGrid_count=0 -landmarksMap_count=0 beaconMap_count=0 pointsMap_count=1 diff --git a/share/mrpt/config_files/icp-slam/icp-slam_demo_classic_gridmatch.ini b/share/mrpt/config_files/icp-slam/icp-slam_demo_classic_gridmatch.ini index 6fae801c8e..fa3a5ad8d0 100644 --- a/share/mrpt/config_files/icp-slam/icp-slam_demo_classic_gridmatch.ini +++ b/share/mrpt/config_files/icp-slam/icp-slam_demo_classic_gridmatch.ini @@ -68,7 +68,6 @@ matchAgainstTheGrid = true # Creation of maps: occupancyGrid_count=1 gasGrid_count=0 -landmarksMap_count=0 beaconMap_count=0 pointsMap_count=0 // 1 diff --git a/share/mrpt/config_files/pf-localization/localization_demo.ini b/share/mrpt/config_files/pf-localization/localization_demo.ini index ad33aefce2..17d7f70c5d 100644 --- a/share/mrpt/config_files/pf-localization/localization_demo.ini +++ b/share/mrpt/config_files/pf-localization/localization_demo.ini @@ -109,7 +109,6 @@ SHOW_PROGRESS_3D_REAL_TIME = true # Creation of maps: occupancyGrid_count=1 gasGrid_count=0 -landmarksMap_count=0 pointsMap_count=0 beaconMap_count=0 diff --git a/share/mrpt/config_files/pf-localization/localization_demo_obsonly_rawlog.ini b/share/mrpt/config_files/pf-localization/localization_demo_obsonly_rawlog.ini index 281d42b7c9..6d6557d4f6 100644 --- a/share/mrpt/config_files/pf-localization/localization_demo_obsonly_rawlog.ini +++ b/share/mrpt/config_files/pf-localization/localization_demo_obsonly_rawlog.ini @@ -110,7 +110,6 @@ SHOW_PROGRESS_3D_REAL_TIME = true # Creation of maps: occupancyGrid_count=1 gasGrid_count=0 -landmarksMap_count=0 pointsMap_count=0 beaconMap_count=0 diff --git a/share/mrpt/config_files/rbpf-slam/RO-SLAM_simulatedData_MC.ini b/share/mrpt/config_files/rbpf-slam/RO-SLAM_simulatedData_MC.ini index 9bed523cbd..9759d8cea5 100644 --- a/share/mrpt/config_files/rbpf-slam/RO-SLAM_simulatedData_MC.ini +++ b/share/mrpt/config_files/rbpf-slam/RO-SLAM_simulatedData_MC.ini @@ -74,7 +74,6 @@ BETA=0.50 // Resampling ESS threshold # Creation of maps: occupancyGrid_count=0 gasGrid_count=0 -landmarksMap_count=0 pointsMap_count=0 beaconMap_count=1 diff --git a/share/mrpt/config_files/rbpf-slam/RO-SLAM_simulatedData_SOG.ini b/share/mrpt/config_files/rbpf-slam/RO-SLAM_simulatedData_SOG.ini index 15aa148ebb..722bef965c 100644 --- a/share/mrpt/config_files/rbpf-slam/RO-SLAM_simulatedData_SOG.ini +++ b/share/mrpt/config_files/rbpf-slam/RO-SLAM_simulatedData_SOG.ini @@ -74,7 +74,6 @@ BETA=0.50 // Resampling ESS threshold # Creation of maps: occupancyGrid_count=0 gasGrid_count=0 -landmarksMap_count=0 pointsMap_count=0 beaconMap_count=1 diff --git a/share/mrpt/config_files/rbpf-slam/example_3_gridmaps.ini b/share/mrpt/config_files/rbpf-slam/example_3_gridmaps.ini index d430e19b08..68a3005eaf 100644 --- a/share/mrpt/config_files/rbpf-slam/example_3_gridmaps.ini +++ b/share/mrpt/config_files/rbpf-slam/example_3_gridmaps.ini @@ -70,7 +70,6 @@ BETA=0.50 // Resampling ESS threshold # Creation of maps: occupancyGrid_count=3 gasGrid_count=0 -landmarksMap_count=0 beaconMap_count=0 pointsMap_count=0 diff --git a/share/mrpt/config_files/rbpf-slam/gas_mapping_2lasers_2enoses.ini b/share/mrpt/config_files/rbpf-slam/gas_mapping_2lasers_2enoses.ini index 340713b751..3f85cecf52 100644 --- a/share/mrpt/config_files/rbpf-slam/gas_mapping_2lasers_2enoses.ini +++ b/share/mrpt/config_files/rbpf-slam/gas_mapping_2lasers_2enoses.ini @@ -75,7 +75,6 @@ BETA=0.50 // Resampling ESS threshold # Creation of maps: occupancyGrid_count=2 gasGrid_count=1 -landmarksMap_count=0 pointsMap_count=0 beaconMap_count=0 diff --git a/share/mrpt/config_files/rbpf-slam/gridmapping_RBPF_ICPbased_intel.ini b/share/mrpt/config_files/rbpf-slam/gridmapping_RBPF_ICPbased_intel.ini index 0bd574786f..3795d4dbde 100644 --- a/share/mrpt/config_files/rbpf-slam/gridmapping_RBPF_ICPbased_intel.ini +++ b/share/mrpt/config_files/rbpf-slam/gridmapping_RBPF_ICPbased_intel.ini @@ -68,7 +68,6 @@ BETA=0.50 // Resampling ESS threshold # Creation of maps: occupancyGrid_count = 0 gasGrid_count = 0 -landmarksMap_count = 0 beaconMap_count = 0 pointsMap_count = 1 diff --git a/share/mrpt/config_files/rbpf-slam/gridmapping_RBPF_ICPbased_malaga.ini b/share/mrpt/config_files/rbpf-slam/gridmapping_RBPF_ICPbased_malaga.ini index 03c4816ab6..e5d984c779 100644 --- a/share/mrpt/config_files/rbpf-slam/gridmapping_RBPF_ICPbased_malaga.ini +++ b/share/mrpt/config_files/rbpf-slam/gridmapping_RBPF_ICPbased_malaga.ini @@ -68,7 +68,6 @@ BETA=0.50 // Resampling ESS threshold # Creation of maps: occupancyGrid_count = 0 gasGrid_count = 0 -landmarksMap_count = 0 beaconMap_count = 0 pointsMap_count = 1 diff --git a/share/mrpt/config_files/rbpf-slam/gridmapping_RBPF_grid_ICPbased_malaga.ini b/share/mrpt/config_files/rbpf-slam/gridmapping_RBPF_grid_ICPbased_malaga.ini index f5820ffadb..c421c8a45a 100644 --- a/share/mrpt/config_files/rbpf-slam/gridmapping_RBPF_grid_ICPbased_malaga.ini +++ b/share/mrpt/config_files/rbpf-slam/gridmapping_RBPF_grid_ICPbased_malaga.ini @@ -63,7 +63,6 @@ BETA=0.50 // Resampling ESS threshold # Creation of maps: occupancyGrid_count = 1 gasGrid_count = 0 -landmarksMap_count = 0 beaconMap_count = 0 pointsMap_count = 0 diff --git a/share/mrpt/config_files/rbpf-slam/gridmapping_intel.ini b/share/mrpt/config_files/rbpf-slam/gridmapping_intel.ini index 1b3a6bbcd5..8874c6924c 100644 --- a/share/mrpt/config_files/rbpf-slam/gridmapping_intel.ini +++ b/share/mrpt/config_files/rbpf-slam/gridmapping_intel.ini @@ -71,7 +71,6 @@ max_loglikelihood_dyn_range = 20 # Creation of maps: occupancyGrid_count=1 gasGrid_count=0 -landmarksMap_count=0 beaconMap_count=0 pointsMap_count=0 diff --git a/share/mrpt/config_files/rbpf-slam/gridmapping_optimal_sampling.ini b/share/mrpt/config_files/rbpf-slam/gridmapping_optimal_sampling.ini index 03c5c17316..5c23ad3ae4 100644 --- a/share/mrpt/config_files/rbpf-slam/gridmapping_optimal_sampling.ini +++ b/share/mrpt/config_files/rbpf-slam/gridmapping_optimal_sampling.ini @@ -69,7 +69,6 @@ BETA=0.50 // Resampling ESS threshold # Creation of maps: occupancyGrid_count=1 gasGrid_count=0 -landmarksMap_count=0 beaconMap_count=0 pointsMap_count=0 diff --git a/tests/montecarlo_test1.ini b/tests/montecarlo_test1.ini index d2a3691841..74158cda36 100644 --- a/tests/montecarlo_test1.ini +++ b/tests/montecarlo_test1.ini @@ -2,7 +2,7 @@ // Config file for the application PF Localization // // ~ The MRPT Library ~ -// Jose Luis Blanco Claraco © 2005-2007 +// Jose Luis Blanco Claraco � 2005-2007 //------------------------------------------------------ //--------------------------------------------------------------------------- @@ -91,7 +91,6 @@ init_PDF_max_y=-5 // Creation of maps: occupancyGrid_count=1 gasGrid_count=0 -landmarksMap_count=0 pointsMap_count=0 beaconMap_count=0 From 5416b69a1a09dc51c3979cddcf09c38295d7faa5 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sat, 18 Nov 2023 01:21:00 +0100 Subject: [PATCH 33/36] CMetricMap::loadFromSimpleMap() now works for delayed-load datasets --- .../observations2map_main.cpp | 29 +++++++++++-------- doc/source/doxygen-docs/changelog.md | 2 ++ libs/obs/src/CMetricMap.cpp | 14 ++++++--- 3 files changed, 29 insertions(+), 16 deletions(-) diff --git a/apps/observations2map/observations2map_main.cpp b/apps/observations2map/observations2map_main.cpp index 757150c693..30ad1cb4ca 100644 --- a/apps/observations2map/observations2map_main.cpp +++ b/apps/observations2map/observations2map_main.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include using namespace mrpt; @@ -61,8 +62,7 @@ int main(int argc, char** argv) cout << " Default: INI_FILE_SECTION_NAME = MappingApplication" << endl; cout << "Push any key to exit..." << endl; - os::getch(); - return -1; + return 1; } string configFile = std::string(argv[1]); @@ -87,7 +87,7 @@ int main(int argc, char** argv) metricMap.setListOfMaps(mapCfg); // Build metric maps: - cout << "Building metric maps..."; + cout << "Building metric maps..." << std::endl; metricMap.loadFromProbabilisticPosesAndObservations(simplemap); @@ -97,20 +97,25 @@ int main(int argc, char** argv) // --------------------------- metricMap.saveMetricMapRepresentationToFile(outprefix); - // grid maps: - for (unsigned int i = 0; - i < metricMap.countMapsByClass(); i++) + // And as binary serialized files: + // ------------------------------------- + for (unsigned int i = 0; i < metricMap.maps.size(); i++) { using namespace std::string_literals; - const auto str = outprefix + "_gridmap_no"s + - mrpt::format("%02u", i) + ".gridmap"s; - cout << "Saving gridmap #" << i << " to " << str << endl; + + const auto& m = metricMap.maps.at(i); + + const auto str = outprefix + mrpt::format("_%02u_", i) + + mrpt::system::fileNameStripInvalidChars( + m->GetRuntimeClass()->className) + + ".bin"s; + + std::cout << "Saving map #" << i << " to " << str << std::endl; CFileGZOutputStream fo(str); - mrpt::serialization::archiveFrom(fo) - << *metricMap.mapByClass(i); + mrpt::serialization::archiveFrom(fo) << m; - cout << "done." << endl; + cout << "Done." << endl; } return 0; diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 8ad8b088c6..7890265916 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -16,6 +16,8 @@ - mrpt::maps::TSetOfMetricMapInitializers::loadFromConfigFile() now throws if it finds a `*_count` entry with an unknown map class name. - \ref mrpt_math_grp - New template mrpt::math::confidenceIntervalsFromHistogram() + - \ref mrpt_obs_grp + - mrpt::maps::CMetricMap::loadFromSimpleMap() now automatically calls mrpt::obs::CObservation::load() and mrpt::obs::CObservation::unload() for all observations, so it works with lazy-load datasets. - BUG FIXES: - Fix compilation errors if using the `MCP_SAVE()` macro with class enum types. - Fix wrong cloud pose in CPointsMap::insertObservation() when inserting an mrpt::obs::CObservationPointCloud. diff --git a/libs/obs/src/CMetricMap.cpp b/libs/obs/src/CMetricMap.cpp index 6bafee74fa..39a6a22244 100644 --- a/libs/obs/src/CMetricMap.cpp +++ b/libs/obs/src/CMetricMap.cpp @@ -40,12 +40,18 @@ void CMetricMap::loadFromProbabilisticPosesAndObservations( this->clear(); // Insert new content: - for (const auto& pair : sfSeq) + for (const auto& [pose, sf] : sfSeq) { - ASSERTMSG_(pair.pose, "Input map has an empty `CPose3DPDF` ptr"); - ASSERTMSG_(pair.sf, "Input map has an empty `CSensoryFrame` ptr"); + ASSERTMSG_(pose, "Input map has an empty `CPose3DPDF` ptr"); + ASSERTMSG_(sf, "Input map has an empty `CSensoryFrame` ptr"); - pair.sf->insertObservationsInto(*this, pair.pose->getMeanVal()); + for (auto& o : *sf) + o->load(); + + sf->insertObservationsInto(*this, pose->getMeanVal()); + + for (auto& o : *sf) + o->unload(); } } From 1d3156000bf3d4f9cf0c1202950a1368d5be1e49 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 20 Nov 2023 19:20:05 +0100 Subject: [PATCH 34/36] Fix potential data race in WorkerThreadsPool::pendingTasks() --- doc/source/doxygen-docs/changelog.md | 1 + libs/core/src/WorkerThreadsPool.cpp | 3 +++ 2 files changed, 4 insertions(+) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 7890265916..6063a86279 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -21,6 +21,7 @@ - BUG FIXES: - Fix compilation errors if using the `MCP_SAVE()` macro with class enum types. - Fix wrong cloud pose in CPointsMap::insertObservation() when inserting an mrpt::obs::CObservationPointCloud. + - Fix potential data race in mrpt::WorkerThreadsPool::pendingTasks() # Version 2.11.2: Released Oct 25th, 2023 - Changes in libraries: diff --git a/libs/core/src/WorkerThreadsPool.cpp b/libs/core/src/WorkerThreadsPool.cpp index 9ed52d13f3..82ca81e83e 100644 --- a/libs/core/src/WorkerThreadsPool.cpp +++ b/libs/core/src/WorkerThreadsPool.cpp @@ -50,6 +50,9 @@ void WorkerThreadsPool::clear() std::size_t WorkerThreadsPool::pendingTasks() const noexcept { + std::unique_lock lock( + const_cast(this)->queue_mutex_); + return tasks_.size(); } From 9a3e0b12840b1e1669406c321815e3ac5df83dcd Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 20 Nov 2023 19:20:43 +0100 Subject: [PATCH 35/36] NN API: Add "prepare()" calls --- libs/maps/include/mrpt/maps/CPointsMap.h | 2 ++ .../include/mrpt/maps/CVoxelMapOccupancyBase.h | 8 ++++++++ .../mrpt/maps/NearestNeighborsCapable.h | 18 ++++++++++++++++++ libs/maps/src/maps/CPointsMap.cpp | 8 ++++++++ 4 files changed, 36 insertions(+) diff --git a/libs/maps/include/mrpt/maps/CPointsMap.h b/libs/maps/include/mrpt/maps/CPointsMap.h index 30728ef6b6..6fb6c6e42f 100644 --- a/libs/maps/include/mrpt/maps/CPointsMap.h +++ b/libs/maps/include/mrpt/maps/CPointsMap.h @@ -1128,6 +1128,8 @@ class CPointsMap : public CMetricMap, /** @name API of the NearestNeighborsCapable virtual interface @{ */ + void nn_prepare_for_2d_queries() const override; + void nn_prepare_for_3d_queries() const override; [[nodiscard]] bool nn_has_indices_or_ids() const override { return true; } [[nodiscard]] size_t nn_index_count() const override { return size(); } [[nodiscard]] bool nn_single_search( diff --git a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h index fc00eb4139..aa77a509a8 100644 --- a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h +++ b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h @@ -292,6 +292,14 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase, /** @name API of the NearestNeighborsCapable virtual interface @{ */ // See docs in base class + void nn_prepare_for_2d_queries() const override + { + getOccupiedVoxels()->nn_prepare_for_2d_queries(); + } + void nn_prepare_for_3d_queries() const override + { + getOccupiedVoxels()->nn_prepare_for_3d_queries(); + } [[nodiscard]] bool nn_has_indices_or_ids() const override { return getOccupiedVoxels()->nn_has_indices_or_ids(); diff --git a/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h b/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h index 73e08913c1..481ceb9197 100644 --- a/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h +++ b/libs/maps/include/mrpt/maps/NearestNeighborsCapable.h @@ -39,6 +39,24 @@ class NearestNeighborsCapable */ [[nodiscard]] virtual bool nn_has_indices_or_ids() const = 0; + /** Must be called before calls to `nn_*_search()` to ensure the required + * data structures are ready for queries (e.g. KD-trees). Useful in + * multithreading applications. + */ + virtual void nn_prepare_for_2d_queries() const + { + // Default: do nothing + } + + /** Must be called before calls to `nn_*_search()` to ensure the required + * data structures are ready for queries (e.g. KD-trees). Useful in + * multithreading applications. + */ + virtual void nn_prepare_for_3d_queries() const + { + // Default: do nothing + } + /** If nn_has_indices_or_ids() returns `true`, this must return the number * of "points" (or whatever entity) the indices correspond to. Otherwise, * the return value should be ignored. diff --git a/libs/maps/src/maps/CPointsMap.cpp b/libs/maps/src/maps/CPointsMap.cpp index d777e15bc2..98d37051d4 100644 --- a/libs/maps/src/maps/CPointsMap.cpp +++ b/libs/maps/src/maps/CPointsMap.cpp @@ -2101,6 +2101,14 @@ void CPointsMap::loadFromVelodyneScan( // =========== API of the NearestNeighborsCapable virtual interface ====== // See docs in base class +void CPointsMap::nn_prepare_for_2d_queries() const +{ + const_cast(this)->kdTreeEnsureIndexBuilt2D(); +} +void CPointsMap::nn_prepare_for_3d_queries() const +{ + const_cast(this)->kdTreeEnsureIndexBuilt3D(); +} bool CPointsMap::nn_single_search( const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result, float& out_dist_sqr, uint64_t& resultIndexOrID) const From 9fe58d91147220698a6fa0578249d05bafcc9477 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Tue, 21 Nov 2023 22:04:48 +0100 Subject: [PATCH 36/36] changelog --- doc/source/doxygen-docs/changelog.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 6063a86279..4c67f704f0 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,6 +1,6 @@ \page changelog Change Log -# Version 2.11.3: UNRELEASED +# Version 2.11.3: Released Nov 21st, 2023 - Changes in libraries: - \ref mrpt_core_grp - Add the `[[nodiscard]]` attribute to all functions returning a value in ``