Skip to content

Commit

Permalink
NN API: use indices or IDs
Browse files Browse the repository at this point in the history
  • Loading branch information
@jlblancoc
jlblancoc committed Nov 5, 2023
1 parent df98834 commit ab9687c
Show file tree
Hide file tree
Showing 23 changed files with 434 additions and 153 deletions.
14 changes: 7 additions & 7 deletions libs/maps/include/mrpt/maps/COccupancyGridMap2D.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<size_t>& 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<size_t>& 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<mrpt::math::TPoint3Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override;
std::vector<uint64_t>& resultIndicesOrIDs) const override;
void nn_multiple_search(
const mrpt::math::TPoint2Df& query, const size_t N,
std::vector<mrpt::math::TPoint2Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override;
std::vector<uint64_t>& resultIndicesOrIDs) const override;
void nn_radius_search(
const mrpt::math::TPoint3Df& query, const float search_radius_sqr,
std::vector<mrpt::math::TPoint3Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override;
std::vector<uint64_t>& resultIndicesOrIDs) const override;
void nn_radius_search(
const mrpt::math::TPoint2Df& query, const float search_radius_sqr,
std::vector<mrpt::math::TPoint2Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override;
std::vector<uint64_t>& resultIndicesOrIDs) const override;
/** @} */

private:
Expand Down
14 changes: 7 additions & 7 deletions libs/maps/include/mrpt/maps/COccupancyGridMap3D.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<size_t>& 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<size_t>& 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<mrpt::math::TPoint3Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override;
std::vector<uint64_t>& resultIndicesOrIDs) const override;
void nn_multiple_search(
const mrpt::math::TPoint2Df& query, const size_t N,
std::vector<mrpt::math::TPoint2Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override;
std::vector<uint64_t>& resultIndicesOrIDs) const override;
void nn_radius_search(
const mrpt::math::TPoint3Df& query, const float search_radius_sqr,
std::vector<mrpt::math::TPoint3Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override;
std::vector<uint64_t>& resultIndicesOrIDs) const override;
void nn_radius_search(
const mrpt::math::TPoint2Df& query, const float search_radius_sqr,
std::vector<mrpt::math::TPoint2Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override;
std::vector<uint64_t>& resultIndicesOrIDs) const override;
/** @} */

private:
Expand Down
14 changes: 7 additions & 7 deletions libs/maps/include/mrpt/maps/CPointsMap.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<size_t>& 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<size_t>& 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<mrpt::math::TPoint3Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override;
std::vector<uint64_t>& resultIndicesOrIDs) const override;
void nn_multiple_search(
const mrpt::math::TPoint2Df& query, const size_t N,
std::vector<mrpt::math::TPoint2Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override;
std::vector<uint64_t>& resultIndicesOrIDs) const override;
void nn_radius_search(
const mrpt::math::TPoint3Df& query, const float search_radius_sqr,
std::vector<mrpt::math::TPoint3Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override;
std::vector<uint64_t>& resultIndicesOrIDs) const override;
void nn_radius_search(
const mrpt::math::TPoint2Df& query, const float search_radius_sqr,
std::vector<mrpt::math::TPoint2Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override;
std::vector<uint64_t>& resultIndicesOrIDs) const override;
/** @} */

protected:
Expand Down
30 changes: 16 additions & 14 deletions libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -292,63 +292,65 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase<voxel_node_t>,
/** @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
{
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<size_t>& 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<size_t>& 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<mrpt::math::TPoint3Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override
std::vector<uint64_t>& 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<mrpt::math::TPoint2Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override
std::vector<uint64_t>& 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<mrpt::math::TPoint3Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override
std::vector<uint64_t>& 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<mrpt::math::TPoint2Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override
std::vector<uint64_t>& 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);
}
/** @} */

Expand Down
42 changes: 19 additions & 23 deletions libs/maps/include/mrpt/maps/NearestNeighborsCapable.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,6 @@
#include <mrpt/math/TPoint2D.h>
#include <mrpt/math/TPoint3D.h>

#include <optional>

namespace mrpt::maps
{
/** Virtual interface for maps having the capability of searching the closest
Expand All @@ -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;

Expand All @@ -50,44 +51,41 @@ 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<size_t>& 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<size_t>& resultIndex) const = 0;
float& out_dist_sqr, uint64_t& resultIndexOrIDOrID) 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.
* \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<mrpt::math::TPoint3Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const = 0;
std::vector<uint64_t>& resultIndicesOrIDs) const = 0;

/// \overload for 2D points
virtual void nn_multiple_search(
const mrpt::math::TPoint2Df& query, const size_t N,
std::vector<mrpt::math::TPoint2Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const = 0;
std::vector<uint64_t>& resultIndicesOrIDs) const = 0;

/** Radius search for closest 3D points to a given one.
*
Expand All @@ -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<mrpt::math::TPoint3Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const = 0;
std::vector<uint64_t>& resultIndicesOrIDs) const = 0;

/// \overload for 2D points
virtual void nn_radius_search(
const mrpt::math::TPoint2Df& query, const float search_radius_sqr,
std::vector<mrpt::math::TPoint2Df>& results,
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const = 0;
std::vector<uint64_t>& resultIndicesOrIDs) const = 0;

/** @} */
};
Expand Down
Loading

0 comments on commit ab9687c

Please sign in to comment.