Skip to content

Commit

Permalink
NN API: Add support for indices
Browse files Browse the repository at this point in the history
  • Loading branch information
@jlblancoc
jlblancoc committed Nov 5, 2023
1 parent 40221d1 commit 5916980
Show file tree
Hide file tree
Showing 10 changed files with 168 additions and 75 deletions.
20 changes: 14 additions & 6 deletions libs/maps/include/mrpt/maps/COccupancyGridMap2D.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1173,28 +1173,36 @@ class COccupancyGridMap2D
/** @name API of the NearestNeighborsCapable virtual interface
@{ */
// See docs in base class
[[nodiscard]] virtual bool nn_supports_indices() const override
{
return false;
}
[[nodiscard]] bool nn_single_search(
const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result,
float& out_dist_sqr) const override;
float& out_dist_sqr, std::optional<size_t>& resultIndex) const override;
[[nodiscard]] bool nn_single_search(
const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result,
float& out_dist_sqr) const override;
float& out_dist_sqr, std::optional<size_t>& resultIndex) 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) const override;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) 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) const override;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) 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) const override;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) 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) const override;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override;
/** @} */

private:
Expand Down
21 changes: 14 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,29 +400,36 @@ class COccupancyGridMap3D

/** @name API of the NearestNeighborsCapable virtual interface
@{ */
// See docs in base class
[[nodiscard]] virtual bool nn_supports_indices() const override
{
return false;
}
[[nodiscard]] bool nn_single_search(
const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result,
float& out_dist_sqr) const override;
float& out_dist_sqr, std::optional<size_t>& resultIndex) const override;
[[nodiscard]] bool nn_single_search(
const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result,
float& out_dist_sqr) const override;
float& out_dist_sqr, std::optional<size_t>& resultIndex) 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) const override;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) 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) const override;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) 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) const override;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) 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) const override;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override;
/** @} */

private:
Expand Down
21 changes: 14 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,29 +1128,36 @@ class CPointsMap : public CMetricMap,

/** @name API of the NearestNeighborsCapable virtual interface
@{ */
// See docs in base class
[[nodiscard]] virtual bool nn_supports_indices() const override
{
return true;
}
[[nodiscard]] bool nn_single_search(
const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result,
float& out_dist_sqr) const override;
float& out_dist_sqr, std::optional<size_t>& resultIndex) const override;
[[nodiscard]] bool nn_single_search(
const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result,
float& out_dist_sqr) const override;
float& out_dist_sqr, std::optional<size_t>& resultIndex) 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) const override;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) 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) const override;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) 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) const override;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) 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) const override;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override;
/** @} */

protected:
Expand Down
32 changes: 20 additions & 12 deletions libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -292,51 +292,59 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase<voxel_node_t>,
/** @name API of the NearestNeighborsCapable virtual interface
@{ */
// See docs in base class
[[nodiscard]] virtual bool nn_supports_indices() const override
{
return getOccupiedVoxels()->nn_supports_indices();
}
[[nodiscard]] bool nn_single_search(
const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result,
float& out_dist_sqr) const override
float& out_dist_sqr, std::optional<size_t>& resultIndex) const override
{
return getOccupiedVoxels()->nn_single_search(
query, result, out_dist_sqr);
query, result, out_dist_sqr, resultIndex);
}
[[nodiscard]] bool nn_single_search(
const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result,
float& out_dist_sqr) const override
float& out_dist_sqr, std::optional<size_t>& resultIndex) const override
{
return getOccupiedVoxels()->nn_single_search(
query, result, out_dist_sqr);
query, result, out_dist_sqr, resultIndex);
}
void nn_multiple_search(
const mrpt::math::TPoint3Df& query, const size_t N,
std::vector<mrpt::math::TPoint3Df>& results,
std::vector<float>& out_dists_sqr) const override
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override
{
getOccupiedVoxels()->nn_multiple_search(
query, N, results, out_dists_sqr);
query, N, results, out_dists_sqr, resultIndices);
}
void nn_multiple_search(
const mrpt::math::TPoint2Df& query, const size_t N,
std::vector<mrpt::math::TPoint2Df>& results,
std::vector<float>& out_dists_sqr) const override
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override
{
getOccupiedVoxels()->nn_multiple_search(
query, N, results, out_dists_sqr);
query, N, results, out_dists_sqr, resultIndices);
}
void nn_radius_search(
const mrpt::math::TPoint3Df& query, const float search_radius_sqr,
std::vector<mrpt::math::TPoint3Df>& results,
std::vector<float>& out_dists_sqr) const override
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override
{
getOccupiedVoxels()->nn_radius_search(
query, search_radius_sqr, results, out_dists_sqr);
query, search_radius_sqr, results, out_dists_sqr, resultIndices);
}
void nn_radius_search(
const mrpt::math::TPoint2Df& query, const float search_radius_sqr,
std::vector<mrpt::math::TPoint2Df>& results,
std::vector<float>& out_dists_sqr) const override
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const override
{
getOccupiedVoxels()->nn_radius_search(
query, search_radius_sqr, results, out_dists_sqr);
query, search_radius_sqr, results, out_dists_sqr, resultIndices);
}
/** @} */

Expand Down
32 changes: 26 additions & 6 deletions libs/maps/include/mrpt/maps/NearestNeighborsCapable.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,8 @@
#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 @@ -32,41 +34,54 @@ class NearestNeighborsCapable
/** @name API of the NearestNeighborsCapable virtual interface
@{ */

/** Returns true if the rest of `nn_*` methods will populate the indices
* std::optional<> return variables, false otherwise. */
[[nodiscard]] virtual bool nn_supports_indices() const = 0;

/** Search for the closest 3D point to a given one.
*
* \param[in] query The query input point.
* \param[out] result The found closest point.
* \param[out] out_dist_sqr The square Euclidean distance between the query
* and the returned point.
* \param[out] resultIndex Depending on the implementation, here it will be
* returned the index of the result point in the map, or std::nullopt if
* indices are not available.
*
* \return True if successful, false if no point was found.
*/
[[nodiscard]] virtual bool nn_single_search(
const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result,
float& out_dist_sqr) const = 0;
float& out_dist_sqr, std::optional<size_t>& resultIndex) const = 0;

/// \overload for 2D points
[[nodiscard]] virtual bool nn_single_search(
const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result,
float& out_dist_sqr) const = 0;
float& out_dist_sqr, std::optional<size_t>& resultIndex) 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.
*
*/
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) const = 0;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) 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) const = 0;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const = 0;

/** Radius search for closest 3D points to a given one.
*
Expand All @@ -75,17 +90,22 @@ class NearestNeighborsCapable
* \param[out] results The found closest points.
* \param[out] out_dists_sqr The square Euclidean distances between the
* query and the returned point.
* \param[out] resultIndices Depending on the implementation, here it will
* be returned the indices of the result points in the map, or std::nullopt
* if indices are not available.
*/
virtual void nn_radius_search(
const mrpt::math::TPoint3Df& query, const float search_radius_sqr,
std::vector<mrpt::math::TPoint3Df>& results,
std::vector<float>& out_dists_sqr) const = 0;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) 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) const = 0;
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const = 0;

/** @} */
};
Expand Down
28 changes: 18 additions & 10 deletions libs/maps/src/maps/COccupancyGridMap2D_common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -770,46 +770,52 @@ float COccupancyGridMap2D::compute3DMatchingRatio(

bool COccupancyGridMap2D::nn_single_search(
const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result,
float& out_dist_sqr) const
float& out_dist_sqr, std::optional<size_t>& resultIndex) const
{
// delegate to the 2D version:
mrpt::math::TPoint2Df r;
bool res = nn_single_search({query.x, query.y}, r, out_dist_sqr);
bool res =
nn_single_search({query.x, query.y}, r, out_dist_sqr, resultIndex);
result = {r.x, r.y, .0f};
return res;
}
void COccupancyGridMap2D::nn_multiple_search(
const mrpt::math::TPoint3Df& query, const size_t N,
std::vector<mrpt::math::TPoint3Df>& results,
std::vector<float>& out_dists_sqr) const
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const
{
// delegate to the 2D version:
std::vector<mrpt::math::TPoint2Df> r;
nn_multiple_search({query.x, query.y}, N, r, out_dists_sqr);
nn_multiple_search({query.x, query.y}, N, r, out_dists_sqr, resultIndices);
results.resize(r.size());
for (size_t i = 0; i < r.size(); i++)
results[i] = {r[i].x, r[i].y, .0f};
}
void COccupancyGridMap2D::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) const
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const
{
// delegate to the 2D version:
std::vector<mrpt::math::TPoint2Df> r;
nn_radius_search({query.x, query.y}, search_radius_sqr, r, out_dists_sqr);
nn_radius_search(
{query.x, query.y}, search_radius_sqr, r, out_dists_sqr, resultIndices);
results.resize(r.size());
for (size_t i = 0; i < r.size(); i++)
results[i] = {r[i].x, r[i].y, .0f};
}

bool COccupancyGridMap2D::nn_single_search(
const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result,
float& out_dist_sqr) const
float& out_dist_sqr, std::optional<size_t>& resultIndex) const
{
std::vector<mrpt::math::TPoint2Df> r;
std::vector<float> dist_sqr;
nn_multiple_search(query, 1, r, dist_sqr);
std::optional<std::vector<size_t>> resultIndices;
resultIndex.reset(); // not supported in gridmaps
nn_multiple_search(query, 1, r, dist_sqr, resultIndices);
if (r.empty()) return false; // none found
result = r[0];
out_dist_sqr = dist_sqr[0];
Expand All @@ -819,7 +825,8 @@ bool COccupancyGridMap2D::nn_single_search(
void COccupancyGridMap2D::nn_multiple_search(
const mrpt::math::TPoint2Df& query, const size_t N,
std::vector<mrpt::math::TPoint2Df>& results,
std::vector<float>& out_dists_sqr) const
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const
{
results.clear();
results.reserve(N);
Expand Down Expand Up @@ -890,7 +897,8 @@ void COccupancyGridMap2D::nn_multiple_search(
void COccupancyGridMap2D::nn_radius_search(
const mrpt::math::TPoint2Df& query, const float search_radius_sqr,
std::vector<mrpt::math::TPoint2Df>& results,
std::vector<float>& out_dists_sqr) const
std::vector<float>& out_dists_sqr,
std::optional<std::vector<size_t>>& resultIndices) const
{
results.clear();
out_dists_sqr.clear();
Expand Down
Loading

0 comments on commit 5916980

Please sign in to comment.