Fix/neighbor_search (#3)

* add voxel index lookup from position * fix neighbor search and augment utest
MIT-SPARK · Jun 18, 2024 · d14349b · d14349b
1 parent a010440
commit d14349b
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 32 deletions.
diff --git a/include/spatial_hash/impl/neighbor_utils_impl.h b/include/spatial_hash/impl/neighbor_utils_impl.h
@@ -42,18 +42,17 @@
 namespace spatial_hash {
 
 template <typename BlockT>
-BlockNeighborSearch<BlockT>::BlockNeighborSearch(const Layer<BlockT>& layer,
-                                                 Connectivity connectivity)
+BlockNeighborSearch<BlockT>::BlockNeighborSearch(const Layer<BlockT>& layer, size_t connectivity)
     : NeighborSearch(connectivity), layer_(layer) {}
 
 template <typename IndexT>
 std::vector<IndexT> NeighborSearch::neighborIndices(const IndexT& index,
                                                     const bool include_self) const {
-  std::vector<IndexT> neighbors(static_cast<size_t>(connectivity) +
-                                static_cast<size_t>(include_self));
-
-  for (size_t i = include_self ? 0 : 1; i < neighbors.size(); ++i) {
-    neighbors[i] = index + kNeighborOffsets.col(i).cast<typename IndexT::Scalar>();
+  const size_t offset = include_self ? 0 : 1;
+  std::vector<IndexT> neighbors;
+  neighbors.reserve(connectivity + offset);
+  for (size_t i = offset; i <= connectivity; ++i) {
+    neighbors.emplace_back(index + kNeighborOffsets.col(i).cast<typename IndexT::Scalar>());
   }
   return neighbors;
 }
@@ -64,7 +63,6 @@ std::vector<const BlockT*> BlockNeighborSearch<BlockT>::neighborBlocks(
     const bool include_self) const {
   std::vector<const BlockT*> neighbors;
   neighbors.reserve(static_cast<size_t>(connectivity) + static_cast<size_t>(include_self));
-
   for (const auto& index : neighborIndices(block_index, include_self)) {
     const auto block = layer_.getBlockPtr(index);
     if (block) {
@@ -77,7 +75,7 @@ std::vector<const BlockT*> BlockNeighborSearch<BlockT>::neighborBlocks(
 
 template <typename BlockT>
 VoxelNeighborSearch<BlockT>::VoxelNeighborSearch(const VoxelLayer<BlockT>& layer,
-                                                 Connectivity connectivity)
+                                                 size_t connectivity)
     : NeighborSearch(connectivity), layer_(layer) {}
 
 template <typename BlockT>

diff --git a/include/spatial_hash/neighbor_utils.h b/include/spatial_hash/neighbor_utils.h
@@ -47,12 +47,8 @@ namespace spatial_hash {
  * @brief Index-based neighbor search.
  */
 struct NeighborSearch {
-  // Number of neighbors to search for.
-  enum class Connectivity : uint32_t { k6 = 6u, k18 = 18u, k26 = 26u };
-
   // Constructors.
-  explicit NeighborSearch(Connectivity connectivity);
-  explicit NeighborSearch(uint32_t connectivity);
+  explicit NeighborSearch(size_t connectivity);
   virtual ~NeighborSearch() = default;
 
   /**
@@ -63,7 +59,7 @@ struct NeighborSearch {
   template <typename IndexT>
   std::vector<IndexT> neighborIndices(const IndexT& index, const bool include_self = false) const;
 
-  const Connectivity connectivity;
+  const size_t connectivity;
 
  protected:
   // Neighbor offsets ordered for self->6->18->26 connectivity.
@@ -76,10 +72,7 @@ struct NeighborSearch {
  */
 template <typename BlockT>
 struct BlockNeighborSearch : public NeighborSearch {
-  BlockNeighborSearch(const Layer<BlockT>& layer, Connectivity connectivity);
-  BlockNeighborSearch(const Layer<BlockT>& layer, uint32_t connectivity)
-      : BlockNeighborSearch(layer, static_cast<Connectivity>(connectivity)) {}
-
+  BlockNeighborSearch(const Layer<BlockT>& layer, size_t connectivity);
   virtual ~BlockNeighborSearch() = default;
 
   /**
@@ -104,9 +97,7 @@ template <typename BlockT>
 struct VoxelNeighborSearch : public NeighborSearch {
   using VoxelType = typename BlockT::VoxelType;
 
-  VoxelNeighborSearch(const VoxelLayer<BlockT>& layer, Connectivity connectivity);
-  VoxelNeighborSearch(const VoxelLayer<BlockT>& layer, uint32_t connectivity)
-      : VoxelNeighborSearch(layer, static_cast<Connectivity>(connectivity)) {}
+  VoxelNeighborSearch(const VoxelLayer<BlockT>& layer, size_t connectivity);
 
   virtual ~VoxelNeighborSearch() = default;
 

diff --git a/src/neighbor_utils.cpp b/src/neighbor_utils.cpp
@@ -36,19 +36,13 @@
 
 namespace spatial_hash {
 
-NeighborSearch::NeighborSearch(Connectivity connectivity) : connectivity(connectivity) {
-  checkConnectivity();
-}
-
-NeighborSearch::NeighborSearch(uint32_t connectivity)
-    : connectivity(static_cast<Connectivity>(connectivity)) {
+NeighborSearch::NeighborSearch(size_t connectivity) : connectivity(connectivity) {
   checkConnectivity();
 }
 
 void NeighborSearch::checkConnectivity() const {
-  CHECK(connectivity == Connectivity::k6 || connectivity == Connectivity::k18 ||
-        connectivity == Connectivity::k26)
-      << "Invalid connectivity value: " << static_cast<uint32_t>(connectivity);
+  CHECK(connectivity == 6 || connectivity == 18 || connectivity == 26)
+      << "Invalid connectivity value: " << connectivity << ", must be 6, 18, or 26.";
 }
 
 const Eigen::Matrix<int, 3, 27> NeighborSearch::kNeighborOffsets = [] {

diff --git a/tests/utest_neighbor_utils.cpp b/tests/utest_neighbor_utils.cpp
@@ -44,11 +44,18 @@ TEST(NeighborUtils, IndicesValid) {
   const auto neighbors = search.neighborIndices(index, true);
   EXPECT_EQ(7, neighbors.size());
   EXPECT_EQ(index, neighbors[0]);
+  EXPECT_EQ(Index(-1, 0, 0), neighbors[1]);
+  EXPECT_EQ(Index(1, 0, 0), neighbors[2]);
+  EXPECT_EQ(Index(0, -1, 0), neighbors[3]);
+  EXPECT_EQ(Index(0, 1, 0), neighbors[4]);
+  EXPECT_EQ(Index(0, 0, -1), neighbors[5]);
+  EXPECT_EQ(Index(0, 0, 1), neighbors[6]);
 
   // Long Index search.
   const LongIndex index2(1, 1, 1);
   const auto neighbors2 = search.neighborIndices(index2, false);
   EXPECT_EQ(6, neighbors2.size());
+  EXPECT_EQ(LongIndex(0, 1, 1), neighbors2[0]);
 
   // 18-neighbor search.
   const NeighborSearch search2(18);
@@ -57,7 +64,7 @@ TEST(NeighborUtils, IndicesValid) {
 
   // 26 neighbor search.
   const NeighborSearch search3(26);
-  const auto neighbors4 = search3.neighborIndices(index, false);
+  const auto neighbors4 = search3.neighborIndices(Index(0, 2, -2), false);
   EXPECT_EQ(26, neighbors4.size());
 }