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: ...